はじめに

ROS（Robot Operating System）のプログラミングは、とても簡単です。その一番大きな理由は、ROSはOSと銘打っているけど実はただの通信ライブラリで、規模が小さいので習得が容易なこと。次に挙げられるのは、ツールやパッケージが揃っているので、我々がプログラミングしなければならない部分が小さいこと。あと、ROSの主たる開発言語のC++が順調にバージョン・アップして、とても使いやすくなったこと。

公平を期すために、ROSプログラミングの問題点についても挙げてみましょう。ドキュメントに載っているサンプルがC++の古い規格（C++03）で書かれているので、プログラミングが面倒くさそうに見えてしまうこと。最新であるC++14を使って簡潔に簡単にプログラミングする場合向けのサンプルは、見つからないんですよね。他は、文書、特に日本語の文書が少ないこと¹。ただでさえ少ない文書が要素技術単位で細切れになっていて、開発の全体像を掴みづらいとも感じます。

簡単に使えてとても便利なROSなのに、もしこの程度の問題で嫌われてしまったらもったいない。だから、C++14を使用したサンプルを作成してきました。そして本稿で、ROSプログラミングについて、実際の開発に則した形で解説します²。

ロボットを動かしてみる

まずは肩慣らし。ロボットを動かしてみましょう。

TurtleBot

本稿は、ロボットとしてTurtleBotを使用します。TurtleBotはROSアプリケーションの開発プラットフォームで、iRobot社のCreateやYujin Robot社のKobuki、Microsoft社のKinect、Asux社のXtion PRO LIVEといった市販のハードウェアで構成される、オープン・ソース・ハードウェアのロボットです。

なぜ、TurtleBot？

本稿がTurtleBotを使用する理由は、安くて簡単で情報が豊富なためです。

安い

20万円程度で購入できます。もしiRobot社のルンバの中古品とXbox360から取り外したKinect、ベニヤ板があるなら、公開された設計書に従って自作することもできます。

簡単

TurtleBotに対応した多数のROSパッケージがあるので、開発範囲を小さくできます。たとえば、ROS WikiのTurtleBotのページの記述通りにコマンド入力すれば、プログラミングせずとも地図作成や自律移動ができます。

情報が豊富

多数の開発者が、安くて簡単なTurtleBotでROSアプリケーションを開発しています。だから、情報も豊富です。たとえばLearn Turtlebot and ROSでは、セットアップからスマートフォンで呼び出し可能なTurtleBotによるコーヒー配膳システムの完成まで、丁寧に解説してくれています。トラブルが発生しても、インターネットを検索すれば解決方法が見つかりますし。

セットアップ

TurtleBot Installationに従い、TurtleBotに必要なソフトウェアをセットアップしてください。

実機がない場合は、シミュレーターで

TurtleBotを用意できなくても、ご安心ください。ROSに対応した、Gazeboというシミュレーターがあります。Gazebo Bringup Guideに従って、TurtleBotのシミュレーターを起動してみてください。内蔵された物理エンジンが、いい感じに物理演算してくれます。

一点だけ、気をつけてください。Gezeboは初回起動時にオブジェクトのデータをダウンロードしますから、画面が表示されるまでかなり時間がかかります。気長に待っていただければ以下の画面が表示され、TurtleBotの実機がなくてもROSを試せるようになります。

ROSの仕組み

プログラミングに入る前に、簡単にROSについて説明させてください。ROSはただの通信ライブラリなのですけれど、どんな通信をしてどのようにロボットを動かすのか、そもそもどうして通信ライブラリがロボットのソフトウェア開発に必要になるのかが分からないと、モヤモヤしますもんね。

ロボットのソフトウェア開発と通信

少し遠回りになりますけど、電子工作相当の低水準の話から。ロボットはセンサーやモーターで構成されているわけですけど、これらはアナログな電気で制御されます。センサーはその測定結果を電圧として渡してきますから、専用の回路でデジタルに変換しなければなりません。DCモーター（普通のモーター）の力の制御も電圧で、でも、電子回路で電圧そのものを制御するのは難しい。だから、PWM（Pulse Width Modulation）制御と呼ばれるスイッチを高速にオン/オフする方式で、DCモーターの力を調節しなければなりません。

で、これがやたらと面倒くさい。だって、高速でスイッチをオン/オフするには、CPUのタイマー割り込みを使った面倒なコードが必要なのですから。なので、例えば電子工作で人気のArduinoでは、analogWrite()というPWM制御用のライブラリを用意し、タイマー割り込みを隠蔽しています。サーボ・モーター（ロボット・アームの関節等で使われる、角度を指定できるモーター）のようにスイッチをオン/オフする周波数が定められている場合向けに、一つのタイマーで複数のサーボ・モーターを制御するServoクラスも用意されています。

でも、ライブラリが揃っても、まだ他に問題があるんです。ロボット・プログラムの動作は「センサーの値を読む→読み取った値に合わせてモーター等を制御する」の繰り返しで、それをコードで表現すると以下になります。

Rate loop_rate(30)  // 30Hz（秒に30回）でループできるように、ループ・レートを設定。
while (1) {  // 無限ループ。
  auto accelerator_opening = analogRead(ACCELERATOR_PIN);  // アクセル開度のセンサーから情報を取得。
  analogWrite(MOTOR_PIN, accelerator_opening);  // モーターの速度を設定。
  
  loop_rate.sleep();  // ループ・レートに合わせてスリープする。
}

上のコードは、一定の周期でループが回るようになっています。たとえば秒30コマ撮るビデオ・カメラのデータを使うプログラムなら秒30回処理が動けば十分なので、ループを30Hzに制限します。でも、この周期って、デバイスによって違うんですよ。たとえば、細かい制御が可能なブラシレス・モーター³制御の最小タイミングはモーターが少し回転するたびで、だから、はるかに大きな周波数を設定しなければなりません。でも、センサーやモーターが増えると複数の周波数に対応しなければならなくなって、これを一つのループで表現するとコードが複雑になるのでやりたくない。PWM制御ライブラリのようなタイマー割り込みでの実現も、作成コストが高いから避けたい。マルチ・スレッドで複数のループを作る方式は、制御が不正確になってしまうのでダメ。

だから一般に、この複数の周波数問題は、CPUを分けることで解決します。CPUと言ってもPCに入ってるIntel Core i7とかの高級品ではなくて、16MHz動作の8bitでメモリ2KB（単位に注意してください）とかの、マイクロ・コントローラーと呼ばれる安価なやつです。これを、センサーやモーター毎につけて、それぞれのデバイスに適した周波数でループを回して制御すればよい。ロボットを分散型で制御するわけですね。

でも、センサーとマイクロ・コントローラー、モーターとマイクロ・コントローラーのような小さな単位を組み合わせて大きなロボットを作るのは、組み合わせが多すぎるので大変。だから、センサーやモーターのマイクロ・コントローラーを、デジタルなので管理が容易なシリアル通信でつなげて、移動ユニットのような形でコンポーネント化します。このコンポーネントの組み合わなら、ロボットの作成は楽になりそう……なのですけれど、残念ですけどまだ課題があります。

というのも、シリアル通信ではビット列を送受信しますが、そのビット列にどのようなデータを使うのかは利用者の自由なんです。だから、A社の移動ユニットとB社の移動ユニットではシリアル通信する内容が異なっていて、コンポーネントを組み合わせたり交換したりはできません。あと、人工知能が行き先を示すようなハードウェア無関係のソフトウェア・コンポーネントでも、問題が発生します。自由度が高いソフトウェア・コンポーネント同士を自由度が低いシリアルで通信させるのは無意味ですから。というわけで、通信はもっと抽象度が高いネットワークで実現すべきでしょう。

まぁ、ネットワーク通信でも、HTTPでどんな内容のホームページでも送信できるように、自由なデータを送れちゃう。でも、ソフトウェアで階層的に実現されている層が多いですから、送信内容の規格化は容易です。各種業界団体が専用のXML規格を定めるように、移動を表現するデータ構造（X軸とY軸とZ軸の移動量と、X軸とY軸とZ軸の回転量）を定めてあげれば、A社の移動ユニットとB社の移動ユニットが交換可能になります。

でも、先ほど例に出てきたHTTPそのものだとセッション単位で1対1の接続なので、センサーから取得した同じデータを複数回送信することになって効率が悪そう。あと、ブラウザからリクエストが来たらデータを送る方式だと、センサーというデータを流し続ける仕組みと合致しない。メッセージがXMLだと、プログラムを書くのが大変そうで嫌。構造体のような、もっとプログラミング言語寄りの楽なやり方が欲しい。うん、やっぱり、既成品じゃあダメですね。

というわけで、ロボット専用の通信ライブラリが必要という結論になりました。ROSは、このロボット専用の通信ライブラリなのです。

Publish/Subscribeの、ロボットに適した通信を実現する

さて、そのロボット専門の通信ライブラリであるROSですが、実はその内部では先ほど文句をつけたHTTPも使用しています。でも、外から見えるROSの動作はHTTPとは大きく異なっていて、さすがはロボット専用な感じ。HTTPはブラウザからGETやPOSTというリクエストを受け取ったらサーバーがコンテンツを送る方式ですけれど、ROSでのサーバーはメッセージ流しっぱ、クライアントが必要に応じて読み取るという方式です。いわゆるPublish/Subscribe。センサーに適した、同じデータが複数回流れない効率のよい動作です。

で、このメッセージを流したり受け取ったりするプログラムの単位をROSではノードと呼び、通常はプロセスとして実現します。あと、種々雑多なメッセージが流れると取捨選択が必要になってしまいますから、同じメッセージをトピックとしてまとめます。このノードやトピックは、管理しやすいように階層で管理します。Microsoft社のKinectを管理する/kinectというノードが、取得した3D点群のメッセージを/3dsensor/depth/pointsというトピックに流す感じ。で、3D点群から障害物を検知してハンドル操作する/steeringノードや、3D点群を後で分析できるようにディスクに保存する/pointcloud_to_pcdノードが、先ほどの/3dsensor/depth/pointsというトピックからメッセージを読み込んでそれぞれの処理を実施します。

この、メッセージをトピックに送ることをpublish、トピックからメッセージを受け取ることをsubscribeと呼びます。出版と購読ですね。先ほどは書きそびれましたけど、3D点群から障害物を検知する/steeringノードは、多分、右へ行けとか左に行けというメッセージをpublishします。で、このメッセージを/move_unitノードがsubscribeする。さらに、/steeringノードは/move_unitノードがpublishする機体の情報をsubscribeして、移動が可能かどうかの判断に使っているでしょう。ROSのノードは対等で、HTTPのようなサーバーとかクライアントという区分けは存在しないんです。

サービスで、1対1の同期通信にも対応する

Publish/Subscribeでのメッセージの流れはパブリッシャーからサブスクライバーへの1方向のみで、非同期です。センサーが情報を垂れ流すロボット制御におけるほとんどの処理はPublish/Subscribeが適しているのですけれど、もしメッセージの内容が変わらないならpublishし続けるのは無駄が多いですし、メッセージを受け取ったかどうかを確認したい場合は非同期だと困ります。

たとえば、ロボットの名前を渡したいとします。名前はいつまでも変わらないですから、名前を何度もpublishするのは無駄です。「あなたの名前は？」「ロボット三等兵です」のようなやりとりの方が、「私の名前はロボット三等兵です」という同じ内容を繰り返し叫び続けるより適切でしょう。

カメラの動作モード設定のような場合も、Publish/Subscribeは使えません。動作モードを切り替えるメッセージをpublishしたけど、/cameraノードはsubscribeしそこねた（RDBMSでトランザクションのようなプログラムを書いている人には信じられないかもしれませんけど、ロボットでは十分にありえる話）。だから、/cameraノードは指定した動作モードとは異なるモードの画像をpublishする。でも、動作モード切り替えを指示したノードは、それを知るすべがない。だから、画像の解析に失敗しちゃう。

というわけで、ROSは、Publish/Subscribe方式に加えてサービスと呼ぶ1対1の同期通信も提供します。残念なことにROSの用語はテキトーで、Publish/Subscribeの場合はトピックとメッセージという用語で、サービスの場合はサービスとサービスという混乱する用語になっています（正しくは、Publish/Subscribeの場合はtopicとmsgで、サービスの場合はserviceとsvcなんですけれど）。だから、わけのわからない説明で申し訳ありませんが、ノードは、サービスからサービス・リクエストを受け取ってサービス・レスポンスを返すことで1対1の同期通信をするという説明になります。

メッセージやサービスを、簡単に定義/使用可能にする

メッセージは*.msg、サービスは*.svcというファイルで定義します。文法は、struct宣言みたいな感じで簡単。そして、*.msgや*.svcはビルド時にC++の構造体のコードに変換されます。普通のコードでメッセージやサービスを作れるわけで、専用のAPIを使わなければならないXML作成より簡単です。トピックにpublishされたメッセージを表示するツールもあって、XMLと同様に人間が目で内容を確認することもできます。

ここまでをまとましょう。ROSプログラムは、ノードと呼ばれるプログラムの集合になります。ノードは、トピックにメッセージをpublishしたりトピックからメッセージをsubscribeしたりして、非同期通信で処理を進めます。あまり使いませんけど、サービスという1対1の同期通信もあります。メッセージやサービスは、struct宣言みたいな感じで定義して、我々が書くC++のコードからはstructに見えるようになります。ほら、「ロボットのソフトウェア開発と通信」で挙げたHTTPとXMLでロボットする場合の問題が、すべて解決されたでしょ？

マルチスレッドとスレッド間通信で、高パフォーマンスも実現する

でも、どれだけ良いものであっても、遅すぎるなら使えません。ROSではノードからノードへメッセージがネットワークで送られていくわけで、しかもそのメッセージには画像とか3D点群とかの大きなデータが含まれるみたい。だとしたら、パフォーマンス面がちょっと不安かも……。

ご安心ください。ROSはパフォーマンスも考慮しています。ROSは、複数のノードをマルチ・スレッドで動作させて、ネットワークではなくメモリでスレッド間通信するNodeletという仕組みも提供しているんです。しかも、コードを修正しなくても、通信相手のノードが別のプロセスや別のコンピューターにあるなら自動でネットワーク通信に切り替えてくれます。これなら、パフォーマンスも安心ですよね。

あと、マルチ・スレッドにまつわる面倒事はNodeletが解消してくれますから、マルチ・スレッドであってもプログラミングは難しくありません。本稿では、この速くて簡単なNodeletを使用してプログラミングしていきます。

ついでに、ディストリビューションとバージョンの話

ROSとROSに対応したパッケージ群を指して、ROSディストリビューションと呼びます。UbuntuやDebianのような、Linuxのディストリビューションと同じ感じ。ディストリビューションの目的は、組み合わせが自由な整合性のあるパッケージのセットを用意することです。

ROSディストリビューションは、毎年新バージョンがリリースされます。本稿を執筆している2016年2月の最新ディストリビューションの名前は「ROS Jade Turtle」で、2016年5月に出る予定の次のディストリビューションは「ROS Kinetic Kame」です。JからKへと、名前の最初のアルファベットがABC順で進んでいきます。なお、ROSディストリビューションのバージョンは、「ROS Jade」や「ROS Kinetic」のように省略して書くことが多いようです。あと、偶数年にリリースされるROSディストリビューションはLTS（Long Term Support）版で、サポート期間が5年間になります（奇数年リリースのサポート期間は2年）。2014年リリースの「ROS Indigo Igloo」と2016年リリース予定の「ROS Kinetic Kame」が、LTSになります。

本稿は、ROS Indigoを使用します。最新のROSディストリビューションへの対応が追いついていないパッケージが多いことと、やはりサポートが長いLTSを使いたいですためです。なお、ROS Wikiの日本語のページでは、ROS Indigo Iglooのさらに前のROS Hydro Medusa向けの記事が多く存在します。ROS Wikiの通りにやってもうまく行かないという場合は、その文書がどのROSディストリビューションを対象にしているのかを確認してみてください。

roslaunch

さて、いよいよプログラミング……の前に、既存のROSプログラムを動かしてみましょう。プログラムは完成したけど動かし方が分からないなんて、笑い話にもならないですもんね。ROSのノードは、直接起動、rosrunで起動、roslaunchで起動の3つの方法で起動できます。本稿のおすすめはroslaunchなのですけれど、その理由を理解していただくためにも、順を追って説明していきます。

直接起動する

通常のROSのノードは、実行可能なプログラムです。だから、直接起動できます。ターミナルを3つ開いて、それぞれで以下のコマンドを実行してください。

• ターミナル#1

$ roscore

• ターミナル#2

$ /opt/ros/indigo/lib/turtlesim/turtlesim_node

• ターミナル#3

$ /opt/ros/indigo/lib/turtlesim/turtle_teleop_key

画面は以下のようになって、turtle_teleop_keyを実行したターミナル#3でカーソルキーを押すと、TurtleSimウィンドウの亀が動きます。

少しだけ解説を。ノード同士が通信するには、相手の情報が必要です。そのために、ROSではMasterというプログラムが動いていて、ノードの情報を集中管理しています。インターネットにおけるDNS（動的な登録ができるので、ダイナミックDNSの方が適切かも）みたいなモノですね。他にも、柔軟なパラメーター管理のために、Parameter Serverというプログラムも動いています。そうそう、ログ管理も。これらのプログラムを1つずつ動かしていくのは大変なので、ターミナル#1で実行したroscoreというコマンドで一気に起動できるようになっています。あと、ターミナル#2と#3で/opt/ros...のように長々と入力しているのは、ROSはプログラムはディレクトリ分割で管理されていて、プログラムのあるディレクトリ群が多すぎてPATH環境変数に登録できないためです。

そうそう、ROSのプログラムはCtrl-c（キーボードのコントロール・キーを押しながらcを押す）で終了できます。起動したプログラムをすべて終了させて、次に進んでください。

rosrunで起動する

上の直接起動する方式だと、パスを入録するのが面倒です。この問題を解決するために、ROSはrosrunというコマンドを提供します。

• ターミナル#1

$ roscore

• ターミナル#2

$ rosrun turtlesim turtlesim_node

• ターミナル#3

$ rosrun turtlesim turtle_teleop_key

これで、少し楽になりました。

roslaunchで起動する

でも、rosrunでもまだまだ面倒くさい。roscoreせずにrosrunするとエラーが表示されますから、roscoreが動いているかどうかは検出できるはず。だったら、何もしなくても必要に応じてroscoreを自動で実行させて欲しい。ターミナルを複数開くのも、コマンドを複数回も入力するのも、終了させるためにCtrl-Cを何度も叩くのも面倒です。

この問題はシェル・スクリプトを書けば解消できそうなのですけれど、できれば、汎用言語であるシェル・スクリプトではなく、ROSに特化した言語で楽にやりたい。Javaプログラムのビルドを、シェル・スクリプトではなくてAntやMaven（.NET FrameworkならMSBuild、RubyならRake、私が大好きなClojureならLeiningen）でやる方が楽なのと同じ。というわけで、roslaunchを使いましょう。roslaunchは、ROSプログラム起動用のコマンドと、そのコマンドで処理可能な言語（フォーマットはXML）の文法の両方を指します。

ただ、これまでで例として使用してきたturtlesimは、roslaunchのlaunchファイルを提供してくれていないんですよ（roslaunchの文法で書かれたファイルをlaunchファイルと呼びます）。なので、本当はセットアップされたパッケージ対してやっていはならないことなのですけれど、launchファイルを追加してみます。

ターミナルで以下のコマンドを実行して、

$ sudo mkdir /opt/ros/indigo/share/turtlesim/launch
$ sudo gedit /opt/ros/indigo/share/turtlesim/launch/teleop.launch

以下の内容のファイルを作成してください⁴。

<launch>
  <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_1"/>
  <node pkg="turtlesim" type="turtle_teleop_key" name="turtlesim_1_teleop_key"/>
</launch>

roslaunchの場合の起動の仕方は、以下の通り。必要なターミナルは一つだけです。

$ roslaunch turtlesim teleop.launch

うん、これなら楽ちんですね⁵。Ctrl-c一回ですべて終了できましたし。

catkin

ついにプログラミング……の前に、パッケージの作成です。ROSアプリケーションは、パッケージと呼ばれる単位で管理されます。我々がこれから作成するROSアプリケーションでも、パッケージのお約束を守らねばなりません。このお約束をうまいこと守りながらビルドしてくれるツールとして、catkinというビルド・ツールがあります。

ワーク・スペースの作成

前にも述べましたが、ROSアプリケーションは、複数のノードがメッセージを送り合うことで動作します。で、これから我々が開発するノードが使用するメッセージは、他のパッケージで定義されたものかもしれません。この「他のパッケージ」をうまいこと利用できるようにする手段が「ワーク・スペース」なのですけど、ROSのプログラミングをまだ始めていない状態ではナンノコッチャって感じ。なので、他のケースで思考実験してみましょう。

題材は、C++のヘッダー・ファイルのインクルードにします。あなたのソース・コードは、もちろん#include <stdio>できます。標準ライブラリのヘッダーですもんね。あと、あなたが今日作成したfor_this_project.hも#include "for_this_project.h"できるでしょう。ファイルをインクルード可能なディレクトリに置くでしょうからね。

でも、昔やったプロジェクトのgood_old_days.hだったら？　この場合は、コンパイラにインクルード・ディレクトリを指定するオプション（-I）を付けなければならないでしょう。でもね、追加したいディレクトリがいっぱいあったら？　オプションを何度も書くのは面倒くさいので、どーにかしたい。幸いなことにROSをインストールしたUbuntuはLinuxなのでコマンドがいっぱいあって大抵のことはできますから、たとえば、

$ g++ `find ~/my_projects \( -name include -a -type d \) -print | sed "s/^/-I/g" | paste -s` x.cpp

と実行すれば、a_long_time_ago.hがmy_projectsの下のプロジェクト用ディレクトリの下のincludeディレクトリにあるなら#include "a_long_time_ago.h"できます。でもまぁ、include以外のフォルダに置かれていると駄目なんですけど……。

はい、思考実験終わり。この思考実験から得られる結論は、同じルールに従うプロジェクトのディレクトリを、あるディレクトリの下にまとめて格納すると便利ってことです。で、catkinのルールに従うパッケージ用のディレクトリを格納して便利にやってやろうぜというディレクトリが、catkinのワーク・スペースというわけ。

では、その便利なワーク・スペースを作りましょう。ターミナルで以下のコマンドを実行することで、ワーク・スペースを作成できます。

$ source /opt/ros/indigo/setup.bash
$ cd <ワーク・スペースの親ディレクトリ>
$ mkdir -p <ワーク・スペース名>/src
$ cd <ワーク・スペース名>/src
$ catkin_init_workspace
$ cd ..
$ catkin_make
$ echo 'source <ワーク・スペースの親ディレクトリ>/<ワーク・スペース名>/devel/setup.bash' >> ~/.bashrc

最後のechoしている1行では、ターミナルを開いたときにこのワーク・スペース向けの環境設定をするように指示しています（お好きなテキスト・エディタで~/.bashrcを編集してもOK）。これをしておかないとせっかく作成したワークスペースが使われませんので、注意してください。

上のコマンド中の、<ワーク・スペースの親ディレクトリ>は、どこでも構いません。私は~/Documents/Projectsの下に開発プロジェクトのファイルを置くと決めているので、ここにしました。<ワーク・スペース名>もなんでも構わないのですが、一般的にcatkin_wsとすることが多いようで、私もこの名前にしました。というわけで、私の環境では~/Documents/Projects/catkin_wsディレクトリがワーク・スペースになりました。以降の記述では、このワーク・スペースを<catkin_ws>と記述します。頭の中で皆様のワーク・スペースに置き換えてくださるよう、お願いいたします。

パッケージの作成

ワーク・スペースができたので、パッケージを作成しましょう。ターミナルから以下のコマンドを実行すれば、パッケージを作成できます。

$ cd <catkin_ws>/src
$ catkin_create_pkg <パッケージ名> [<依存するパッケージ名1> [<依存するパッケージ名2>]]

最初にcdする先は、srcディレクトリです（私の環境では~/Documents/Projects/catkin_ws/src）。catkin_create_pkgをワーク・スペース直下で実行しないように、注意してください（まぁ、間違えて実行しても、生成されたディレクトリを消せば大丈夫なのですけれど）。

2行目のcatkin_create_pkgが、パッケージの雛形生成です。オプションの二番目以降の<依存するパッケージ名>は、そんなの作る前に分かるはずが無いですし、入れると雛形に余計な記述が増えて混乱したりもするので、使うことが確実なC++開発用パッケージであるroscppだけでよいでしょう。

で、さて、必須オプションの<パッケージ名>は、何にしましょうか？　本稿で作成するプログラムは、ロボットの周囲360°の3D点群や対象物の周囲360°からの3D点群を生成するというものです。キーワードは360°ですね。ただ、プログラミングの世界では180°をπにしたラジアンを使うのが一般的なので、2π≒2×3.14＝6.28を名前にしましょう。というわけで、catkin_create_pkg six_point_two_eight roscppしてパッケージを作成してください。

package.xmlとCMakeLists.txtの編集

生成されたパッケージ・ディレクトリをlsしてみると、以下のようになっています。

$ ls -F six_point_two_eight
CMakeLists.txt  include/  package.xml  src/

includeは*.hファイルを格納するディレクトリ、srcは*.cppファイルを格納するディレクトリです。とりあえずこの2つは無視してください。パッケージの情報を表すpackage.xmlファイルと、ビルドを制御するCMakeLists.txtファイルは、今のうちに編集しておきましょう。

package.xml

コメントを削除し、埋めるべきところを埋めると、以下になります。

<?xml version="1.0"?>
<package>
  <name>six_point_two_eight</name>
  <version>0.1.0</version>
  <description>Getting 360 degree PointCloud.</description>
  <maintainer email="rojima1@gmail.com">OJIMA Ryoji</maintainer>
  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>roscpp</build_depend>
  
  <run_depend>roscpp</run_depend>

  <export>
  </export>
</package>

CMakeLists.txt

自動生成されたゴチャゴチャのファイルから必要な部分だけを残し、C++14でのプログラミング向けの要素を追加します。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  roscpp
)

# find_package(Boost REQUIRED COMPONENTS system)

# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

# generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
# )

catkin_package(
#  CATKIN_DEPENDS                          # roscppを削除
)

add_compile_options(                       # 追加
  "-std=c++14"                             # 追加
)                                          # 追加

include_directories(
  include                                  # 追加
  ${catkin_INCLUDE_DIRS}
)

add_library(six_point_two_eight
  src/six_point_two_eight.cpp              # ${PROJECT_NAME}/を削除
)

# add_dependencies(six_point_two_eight ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(six_point_two_eight
  ${catkin_LIBRARIES}
)

add_compile_optionsで、C++14モードでコンパイルするように指示しています。あと、catkin_create_pkgはincludeディレクトリを作ったくせに、catkin_create_pkgが生成したCMakeLists.txtのままだと、インクルード対象のディレクトリになりません……。仕方がないですから、include_directoriesで追加しました。

g++ 4.9

最後に、もう一つだけ準備作業を。ROS Indigoの対象プラットフォームであるUbuntu 14.10のg++のバージョンは4.8で、バージョン4.8だとC++11までしかサポートしないんです……。

なので、以下のコマンドを実行して、g++ 4.9をセットアップしておいてください。

$ sudo add-apt-repository ppa:ubuntu-toolchain-r/test
$ sudo apt-get update
$ sudo apt-get install gcc-4.9 g++-4.9
$ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.9 60 --slave /usr/bin/g++ g++ /usr/bin/g++-4.9
$ sudo update-alternatives --config gcc

Nodelet

やりました。ついに、プログラミングです。

*.hに書く？　*.cppに書く？

回りくどくて申し訳ないのですけど、古き良き時代の話から。C++が生まれる前、C言語の時代の物事は単純で、関数の宣言は*.h、関数の実装は*.cに書くと決まっていました。そして、C++が生まれ、クラス宣言を*.hに書くようになったところで、この美しい原則は壊れました。クラスのメンバー関数はC言語の関数よりも小さな場合があり、通常の関数呼び出しだとプログラムが遅くなるからインライン展開が必要で、その場合の実装は*.hに書くことになっちゃったんですよ。*.hに、宣言に加えて実装（の一部）を書くわけです。その後、ジェネリック・プログラミング（テンプレート）が混乱に拍車をかけました。ジェネリック・プログラミングはコンパイル時にコードを生成する技術で、コード生成には生成元のコードが必要です。コンパイル時には*.hしか参照できませんから、生成元コードは*.hに書くしかない……。というわけで、実装のほとんどが*.hにあるコードが増えてきました。たとえば、線形代数ライブラリのEigenでは、実装のすべてが*.hに入っています。*.hには、それが宣言であれ実装であれ、*.cppで共有しなければならないものは何でも書くことになりました。

では我々も*.hにコードを書けばよいのかというと、ROSの場合はちょっと微妙です。*.hには複数の*.cppで共有する内容を書くべきなのですけれど、でもROSのノードはROS通信で呼び出されます。だから、C++の仕組みでの呼び出しは不要なわけ。だからクラス宣言を*.hで共有する必要はないわけで、だったら全部を*.cppに書いても問題ないはず。折衷案として、インライン展開するかどうかはコンパイラの判断に任せることにして、クラス宣言は*.hに、メンバー関数の実装は*.cppに書くという手もあるのですけれど、それだとジェネック・プログラミングが使えません。ノード間で共有するユーティリティーではテンプレートを使いたいわけで、その場合だけ*.hに実装があるのではコードの閲覧性が悪くなってしまいます。

と、いろいろ考えても明らかな正解はなさそうだったので、実利だけを考え、本稿ではコード量が少なくて閲覧性が高い*.hに実装を書く方式を採用しました。

package.xmlとCMakeLists.txtにnodeletを追加

さて、我々はこれからNodeletを作成するわけですが、そのNodeletは、nodeletパッケージが提供する機能です。でも、我々のsix_point_two_eightパッケージが参照しているのはroscppパッケージだけ。このままではNodeletを作れませんので、nodeletパッケージを参照するようにpackage.xmlとCMakeLists.txtを編集しましょう⁶。

package.xml

<build_depend>にビルド時に必要なパッケージを、<run_depend>に実行時に必要なパッケージを指定します。nodeletパッケージはビルドでも実行でも必要ですから、両方を記述してください。

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>roscpp</build_depend>
  <build_depend>nodelet</build_depend>  <!-- 追加 -->
  
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>      <!-- 追加 -->
  
  <!-- 略 -->
</package>

CMakeLists.txt

find_packageにnodeletを追加するだけです。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  nodelet  # 追加
  roscpp
)

# 略

make_world_models.h

初めてのROSプログラミングですから、Hello, world!をやりましょう。include/six_point_two_eight/make_world_models.hを作成し、以下のコードをタイプしてください。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

namespace six_point_two_eight {
  class MakeWorldModels : public nodelet::Nodelet {
  public:
    void onInit() {
      ROS_INFO_STREAM("Hello, world!");
    }
  };
}

ROS Style Guideでは、ファイル名はunder_scoredとなっています。なので、MakeWorldModelsというクラスのファイル名は、make_world_models.hになりました（本稿のコードの「{」の位置はスタイル・ガイドに準拠していませんけど、個人的に譲れないところなのでご容赦ください。皆様は、スタイル・ガイド準拠でコードを書いてください）。

Nodeletを作る時は、nodelet::Nodeletを継承しなければなりません。あと、初期化処理はonInit()に書きます。今回は、ROSのログ出力であるROS_INFO_STREAMを使用して「Hello, world!」という文字列を出力することにしました。Nodeletのドキュメントを見るとNODELET_INFO_STREAMというNodelet用のログ出力マクロが出てきますけど、Nodeletの外側で使用できなくて不便なので、本稿では使用しません。

そうそう、MakeWorldModelsというクラス名は「ロボットを動かしてみる」という内容と合っていませんが、これは、次の章の「対象物の周囲360°からの3D点群を作成する」につなげたいからなのでご容赦ください。

six_point_two_eight.cpp

*.hをインクルードする*.cppも作成しましょう。NodeletのクラスをエクスポートするにはPLUGINLIB_EXPORT_CLASSというマクロを呼び出しておく必要があって、ドキュメントにはそのマクロのヘッダー・ファイル（pluginlib/class_list_macros.h）は*.cppでインクルードしろと書いてあります。*.cppの記述は単純でクラス単位に分割する必要はなさそうですから、全体で一つ、パッケージ名と同じファイル名にしましょう。

というわけで、src/six_point_two_eight.cppに、以下のコードをタイプしてください。

#include <pluginlib/class_list_macros.h>
#include "six_point_two_eight/make_world_models.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)

コードを作成したら、ビルドです。catkinのビルドは、catkin_ws直下で実行します（ワーク・スペース全体のビルドにしないと、パッケージの整合性をとれませんから。catkin_ws/src/six_point_two_eightでビルドを実行しないように、気をつけてください）。ビルドのためのコマンドは、catkin_makeです。

$ cd <catkin_ws>
$ catkin_make
Base path: /home/ryo/Documents/Projects/catkin_ws
Source space: /home/ryo/Documents/Projects/catkin_ws/src
Build space: /home/ryo/Documents/Projects/catkin_ws/build
Devel space: /home/ryo/Documents/Projects/catkin_ws/devel
Install space: /home/ryo/Documents/Projects/catkin_ws/install
####
#### Running command: "make cmake_check_build_system" in "/home/ryo/Documents/Projects/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/ryo/Documents/Projects/catkin_ws/devel
（後略）

catkin_makeの出力には進捗が％で表示されるのですけれど、それが100%になってエラーが表示されてなければ、ビルド成功です。

six_point_two_eight.xmlと、もう一度package.xml

作成したNodeletには、ROS向けの名前をつけてあげなければなりません。まずは、six_point_two_eight.xmlを作成し、以下のコードをタイプしてください。

<library path="lib/libsix_point_two_eight">
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
</library>

次に、作成したXMLファイルをROSに認識させるために、package.xmlの<export>に1行追記してください。

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  <export>
    <nodelet plugin="${prefix}/six_point_two_eight.xml" />  <!-- 追加 -->
  </export>
</package>

make_world_models.launch

さて、Nodeletはnodeletパッケージのnodeletコマンドを通じて起動するのですけれど、これが実に面倒くさい。だから、前述したroslaunchで起動することにしましょう。launch/make_world_models.launchを作成して、以下をタイプしてください。

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen"/>
</launch>

2行目で、NodeletManagerをプロセスとして起動しています。このNodeletManagerの中に、複数のNodeletを起動するわけです（今回は一つだけですけど）。output="screen"と指定しているのは、ログを画面に出力させたいから。これを書いておかないと、何か問題が起きても画面に何も表示されなくて、開発時にかなり混乱します。

3行目が、作成したNodeletの起動です。args属性のloadの次が起動するNodeletの名前で、その次がNodeletManagerの名前です。今回はNodeletManagerに2行目で起動したsix_point_two_eight_nodelet_managerを指定しましたが、ロボットのドライバ等が作成したNodeletManagerを指定することも可能です。たとえば、TurtleBotの深度センサーのドライバが動作しているcamera/camera_nodelet_managerというNodeletManagerとか。six_point_two_eight_nodelet_managerの代わりにcamera/camera_nodelet_managerを指定すれば、深度センサーからデータを取得する速度が、ネットワーク通信が減る分だけ向上するでしょう。ただし、作成したプログラムの不具合でNodeletManagerが異常終了して深度センサーのドライバまで止まってしまうという危険性もありますから、開発時は専用のNodeletManagerの使用を推奨します。

これですべての作業は完了しましたので、さっそく、我々のNodeletを起動してみましょう。

$ roslaunch six_point_two_eight make_world_models.launch
... logging to /home/ryo/.ros/log/60de7594-df68-11e5-b06d-3b9332081cf6/roslaunch-ryo-T550-17006.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ryo-T550:33789/

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.16

NODES
  /
    make_world_models (nodelet/nodelet)
    six_point_two_eight_nodelet_manager (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [17018]
ROS_MASTER_URI=http://ryo-T550:11311

setting /run_id to 60de7594-df68-11e5-b06d-3b9332081cf6
process[rosout-1]: started with pid [17031]
started core service [/rosout]
process[six_point_two_eight_nodelet_manager-2]: started with pid [17034]
process[make_world_models-3]: started with pid [17044]
[ INFO] [1456807510.442436453]: Loading nodelet /make_world_models of type six_point_two_eight/make_world_models to manager six_point_two_eight_nodelet_manager with the following remappings:
[ INFO] [1456807510.444383031]: waitForService: Service [/six_point_two_eight_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1456807510.482190090]: Initializing nodelet with 4 worker threads.
[ INFO] [1456807510.486318180]: waitForService: Service [/six_point_two_eight_nodelet_manager/load_nodelet] is now available.
[ INFO] [1456807510.538166303]: Hello, world!

うん、最後の行に、「Hello, world!」が出力されました……。

Publisher

「Hello, world!」と画面に出すだけって、ROS関係ないじゃん！　ごめんなさい、おっしゃる通りです。もう少し高度なこと、TurtleBotを前に進ませる処理を書いてみましょう。

トピックとメッセージ

さて、ロボットに命令を出すにはどうすればよいのか？　前述したように、適切なトピックに適切なメッセージをpublishすればよい。でも、ロボットを移動させる場合は、具体的にどのトピックとメッセージを使えばいいの？

それを調べるツールとして、ROSはrostopicとrosmsgを提供しています。試してみましょう。まずは、ターミナルを開いてTurtleBotのドライバを起動してください。

$ roslaunch turtlebot_bringup minimal.launch
... logging to /home/ryo/.ros/log/b43068d0-df70-11e5-b152-630afb514811/roslaunch-ryo-T550-22396.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ryo-T550:45177/

SUMMARY
========

PARAMETERS
 * /app_manager/auto_rapp_installation: False

いろいろメッセージが出て、中にはWarningと書かれていたりもしますけど、エラーが出ない限りは大丈夫です。

もう一つターミナルを開いて、rostopic listしてみてください。

$ rostopic list
ryo@ryo-T550:~/Documents/Projects/catkin_ws$ rostopic list
/capability_server/bonds
/capability_server/events
/cmd_vel_mux/active
/cmd_vel_mux/input/navi
/cmd_vel_mux/input/safety_controller
/cmd_vel_mux/input/teleop
/cmd_vel_mux/parameter_descriptions
/cmd_vel_mux/parameter_updates
/diagnostics
/diagnostics_agg
/diagnostics_toplevel_state
/gateway/force_update
/gateway/gateway_info
/info
/interactions/interactive_clients
/interactions/pairing
/joint_states
/laptop_charge
/mobile_base/commands/controller_info
/mobile_base/commands/digital_output
/mobile_base/commands/external_power
/mobile_base/commands/led1
/mobile_base/commands/led2
/mobile_base/commands/motor_power
/mobile_base/commands/reset_odometry
/mobile_base/commands/sound
/mobile_base/commands/velocity
/mobile_base/controller_info
/mobile_base/debug/raw_control_command
/mobile_base/debug/raw_data_command
/mobile_base/debug/raw_data_stream
/mobile_base/events/bumper
/mobile_base/events/button
/mobile_base/events/cliff
/mobile_base/events/digital_input
/mobile_base/events/power_system
/mobile_base/events/robot_state
/mobile_base/events/wheel_drop
/mobile_base/sensors/bumper_pointcloud
/mobile_base/sensors/core
/mobile_base/sensors/dock_ir
/mobile_base/sensors/imu_data
/mobile_base/sensors/imu_data_raw
/mobile_base/version_info
/mobile_base_nodelet_manager/bond
/odom
/rosout
/rosout_agg
/tf
/tf_static
/turtlebot/incompatible_rapp_list
/turtlebot/rapp_list
/turtlebot/status
/zeroconf/lost_connections
/zeroconf/new_connections

これが、TurtleBotのドライバ起動後にROS上に存在するトピックの一覧です。この中から、適切なトピックを選べばよいというわけ……なんですけど、TurtleBotのドキュメントを見てもトピックの説明は見つかりませんでした。しょうがないので、rosmsgも活用して、アタリをつけていきます。

$ rostopic info /mobile_base/commands/velocity
Type: geometry_msgs/Twist

Publishers: 
 * /mobile_base_nodelet_manager (http://ryo-T550:58177/)

Subscribers: 
 * /mobile_base_nodelet_manager (http://ryo-T550:58177/)

$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

rostopic info <トピック名>で、トピックの情報を表示できます。表示内容で重要なのは1行目のType:で、これで/mobile_base/commands/velocityトピックにはgeometry_msgs/Twistをpublish/subscribeできるということが分かります。で、rosmsg show <メッセージ名>でメッセージの型情報を表示できるので、なんとなく推測ができるというわけ。上の情報から、/mobile_base/commands/velocityトピックがたぶん移動で、geometry_msgs/Twistのlinearが直線移動でangularが回転なんだなぁと推測できます。カン頼みのでたらめな方法ですけど、試して駄目なら別のトピックを調べればいいだけですし、ROSアプリケーションに慣れるとカンが冴えてくるので、それほど大きな問題にはなりません。

別のやり方としては、同じようなことをしているTurtleBot向けROSアプリケーションのコードを調べるという手があります。まずは、ROS.orgのBrowse Softwareでindigoとpackageを選択してテキスト・ボックスに「turtlebot」と入力し、searchボタンをクリックしてください。

検索結果の中から、アプリケーションっぽい感じがするturtlebot_appsをクリック。

今はソース・コードを見たいので、Source:の後ろのhttps://github.com/turtlebot/turtlebot_apps.gitをクリック。

移動に関係がありそうなturtlebot_follower、src、follower.cppと開いて、てコードを読む。

コードを読んでいくと、多分geometry_msgs::Twistが移動用のメッセージなんだろうなぁと推測できます。

もし推測したトピックとメッセージに自信がないなら、rostopic pub <トピック名> <メッセージ名> <YAML形式のデータ>を使って、トピックにメッセージをpublishすることもできます。プログラムを書きだす前に、試してみましょう。

$ rostopic pub /mobile_base/commands/velocity geometry_msgs/Twist '{linear: {x: 0.1}}'

はい、TurtleBotが少し前に動きました。これで、/mobile_base/commands/velocityトピックとgeometry_msgs/TwistメッセージでTurtleBotを動かせることが分かりました。

座標系と単位

プログラミングに入る前に、ROSの座標系について述べさせてください。先ほどのrostopic pubでlinear.xに0.1を指定すると前に進むってのを理解するには、座標系と単位の知識が必要ですもんね。

ROSの座標系は、X軸が前でY軸が左、Z軸が上です。また、X軸を赤、Y軸を緑、Z軸を青で描きます。だから、linear.xにプラスの数値を入れると前に進むというわけ。回転の場合は、座標軸の正の方向を見た場合の時計回り、負の方向に見ると反時計回りになります。だから、Z軸の回転は、ロボットを上から見ると反時計回り。なので、rostopic pub /mobile_base/commands/velocity geometry_msgs/Twist '{angular: {z: 0.5}}'とすると、少しだけ反時計回りします。

あと、距離の単位はメートル、時間の単位は秒、角度の単位はラジアン、周波数はHzです。linear.x = 0.1は、秒速0.1mで前進しろ、angular.z = 0.5は秒0.5ラジアンで反時計回りに回転しろという意味になります。

package.xmlとCMakeLists.txt

さて、移動のメッセージはgeometry_msgs/Twistであることは分かったわけですけど、このメッセージを使うにはpackage.xmlとCMakeLists.txtの修正が必要です。ROSのメッセージ名は<パッケージ名>/<クラス名>に分解できますので、それぞれにgeometry_msgsを追加しましょう。

package.xml

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>geometry_msgs</build_depend>  <!-- 追加 -->
  <build_depend>nodelet</build_depend>
  <build_depend>roscpp</build_depend>
  
  <run_depend>geometry_msgs</run_depend>      <!-- 追加 -->
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>

  <!-- 略 -->
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs  # 追加
  nodelet
  roscpp
)

# find_package(Boost REQUIRED COMPONENTS system)

# 略

make_world_models.hpp

前準備が完了しましたので、プログラミングしましょう。ロボットを前に進ませるコードを追加したinclude/six_point_two_eight/make_world_models.hは、以下の通り。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>

#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    std::thread working_thread_;
    
  public:
    void onInit() {
      // Publisherを生成します。
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);

      // onInitの中でループするとNodeletManagerの処理が止まってしまうので、別スレッドで実行します。
      working_thread_ = std::thread(
        [&]() {
          ros::Rate rate(10);  // 周波数は10Hz（1秒に10回ループします）。
          while (ros::ok()) {  // ROSがシャットダウンされるまで、無限ループします。
            velocity_publisher_.publish(createTwistMsg(0.1, 0.0));  // メッセージを生成して、publishします。
            rate.sleep();  // 適切な周波数になるようにスリープ。
          }
        });
    }
  };
}

geometry_msgs/Twistを使えるようにするために、#include <geometry_msgs/Twist.h>しています。あと、ROSのメッセージには気の利いたコンストラクタが無くて使いづらいので、createTwistMsg()というメッセージ生成用の関数を作りました（TurtleBotは前後進とZ軸の回転しかできないので、linear.xとangular.zのみを設定すれば十分）。メンバー関数ではなくて普通の関数なので、inlineを付加しています。C++14だと、関数の戻り値の型をautoにしておくと型推論してくれてとても便利ですな（型が無いのとは違います。静的な型チェックが実施されますから、コードに型間違いがあればコンパイル・エラーになって安心でもあります）。

publishは、ros::Publisherを使用して実施します。ros::Publisherは、ros::NodeHandleクラスのadvertise<メッセージの型>(トピック名, キューのサイズ)メンバー関数で作成できます。このros::NodeHandleクラスってのはROSノードへのハンドルで、ノードに関する操作はこのNodeHandleを通じて実施します（Nodeletはノードを名乗っていますけど、ROSノードを操作するためのメンバー関数は提供しません……）。で、Nodeletの中でros::NodeHandleを取得するメンバー関数が、上のコードで使用しているgetNodeHandle()なわけです。

getNodeHandle()には、対になるgetMTNodeHandle()というマルチ・スレッド対応バージョンもあるのですけれど、その場合はスレッドの排他制御を自前でプログラミングしなければなりません。getNodeHandle()ならROSからの呼び出しが直列化され、排他制御が不要になってとても楽ちんで安心です。あと、getPrivateNodeHandle()とgetMTPrivateNodeHandle()というPrivate付きのバージョンもあるのですけど、Private付きだと名前解決にノード名が追加されてしまいます（mobile_base/commands/velocityが/make_world_models/mobile_base/commands/velocityと解釈されちゃう⁷）。というわけで、通常はgetNodeHandle()を使ってください。

あとは、作成したros::Publisherのインスタンスが消えてしまわないよう、メンバー変数のvelocity_publisher_に保持しておきます。

onInit()の後半。別スレッドを起こしてループしているところ。なんでこんな面倒なことをしているかを理解していただくために、先ほどのrostopic echoの時のロボットの動きを思い出してください。あの時、ロボットは少しだけ動いてすぐに止まってしまいましたよね？　これは、ロボットは一定周期で指示を受け取る仕組みになっているためです。動けと言われたので1周期分動いて、次の周期には命令が来ていないから動きを止めちゃったってわけ。というわけで、ロボットを動かし続けるためにはループが必要です。

で、getNodeHandle()のところで少し述べたように、Nodeletはスレッドの排他制御が不要になる仕組みを用意しています。そのためには、同じリソースを扱うスレッドを並行に動作させない（直列化して順番に実行させる）ことが有効です。NodeletでgetNodeHandle()を使うと、異なるノードの処理は並行で、同じノードの処理は直列で動作するようにになります。ただし、onInit()はどうやら全ノードで直列らしくて、onInit()の中でループすると他のノードの初期化が始まらなくなってしまいます。というわけで、ループのような時間がかかる処理を実行する場合は、上のコードのようにstd::threadを使用して別スレッドを起こしてあげてください。

最後。std::threadの引数の[&]() { ... }の部分は、C++14のラムダ式です。関数をその場で生成して、std::threadのコンストラクタに渡します。ラムダ式の最初の[]には処理の中で使用するラムダ式の外側の変数を書くのですけど、ここに[&]と書いておくと外側の変数すべてを使用できるようになってとても便利です（だから、ラムダ式の中でvelocity_publisher_が使えています）。

実行する

TurtleBotのノードにメッセージをpublishするプログラムを作成したわけですから、まずはTurtleBotのノードを起動します。ターミナルを開いて、以下のコマンドを実行してください（「トピックとメッセージ」のところで起動済みの場合は、コマンドの実行は不要です）。

$ roslaunch turtlebot_bringup minimal.launch

その上で、別のターミナルでmake_world_models.launchをroslaunchしてください。

$ roslaunch six_point_two_eight make_world_models.launch

これで、TurtleBotが前に動き出すはずです。今回作成したプログラムはひたすらまっすぐに進むだけですから、TurtleBotが壁にぶつかる前にCtrl-cして止めてあげてください。

Timer

まっすぐ進むだけなら、ゼンマイ仕掛けのおもちゃでもできちゃう……。せっかくのROSなのだから、もっと制御したい。というわけで、一定時間後に止まるようにしてみましょう。そのために、ros::Timerを使用します。

make_world_models.hpp

ros::Timerは簡単に使用できますので、いきなりコードで。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;
    
  public:
    void onInit() {
      // Publisherを生成します。
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);

      // ループの代わりにros::Timerを使用します。
      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),  // 0.1秒に一回＝10Hz。
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.1, 0.0));
        });

      // 5秒経ったら、処理を止めます。
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(5.0),
        [&](const auto& message) {
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

ros::Timerは、引数で指定した周期で、引数で指定した処理を呼び出してくれます。ROSでの時刻はros::Timeで時間はros::Durationなので、ros::Timerを作成するcreateTimer()の第一引数はros::Durationです。第二引数は実行する処理で、関数とかクラス・メソッドとかファンクターとか、あとラムダ式が使えます。C++14のラムダ式は、引数の型にもautoが使えてとても便利です。constと&をつけているのは、&だと参照渡しになるのでインスタンスのコピーが不要になって高速化でき、constを付けておけば参照渡しであっても呼び出し側の値が変更されないため値渡しの場合と同じコードを書けるから。最近のC++のライブラリの引数はconst type& varばっかりなわけで、他の書き方だと変な意図があるんじゃないかと混乱させちゃうしね。

今回のコードでは、ros::Publisherの知識だけで書いた先ほどのコードのスレッドとループをros::Timerに置き換えて、timer_1_に格納しています。この書き方の方が楽ちんですよね。あと、移動を停止するのは、移動するようにpublishするタイマーを止めればよいわけですから、5秒後（ros::Duration(5.0)）にtimer_1_.stop()するros::Timerを設定してtimer_2_に格納しています。あと、timer_2_が5秒周期で何回も処理を実施するのは無駄ですから、timer_2_のラムダ式の中ではtimer_2_.stop()も実行しておきます。

はい、お疲れさま。これで、TurtleBotが動いて止まるプログラムが完成しました！　……まぁ、ゼンマイ仕掛けの安物のおもちゃでも、動いて止まるけどな。

ロボットの周囲360°の3D点群を作成する

単純な動きだけじゃツマラナイ！　センサーを使いたい！　了解しました。TurtleBotには深度センサーが付いているので、それを活用して外界データを作成してみましょう。プログラムから、以下の画像のようなデータを作ります。以下の画像のデータはオフィスの私の席の近くの3D点群で、左上で腕をブラブラさせてだらけまくっているのが私になります。もちろん、グリグリ動きます。

PointCloud2

TurtleBotには、Microsft社のKinectかASUS社のXtion PRO LIVEがついています。これらは深度センサーと呼ばれるデバイスで、外界の3D点群を作成してくれます（Kinectというと骨格のイメージがありますけど、骨格はこの3D点群を解析して作成しています）。

深度センサーを扱うプログラム開発時の、TurtleBotとの接続方法

TurtleBot Installationでは、PCを2台用意して、1台をTurtleBotの上に、残り1台を手元に置くようになっています。でも、深度センサーのデータは大きいので、この方式だとフレームレートが低く（私の環境は数秒に1コマ！）なってしまうんですよ。本番環境では深度センサーのデータ処理はTurtleBot上のPCで実行するので問題ないでしょうけど、開発するには問題です。

なので、今回は手元のPCにTurtleBotをつなぐことにしましょう。長めのUSB延長ケーブルを用意して、手元のPCにTurtleBotをつなげてください。

RViz

では、深度センサーのデータがどんなものか、試してみましょう。ROSにはRVizという可視化ツールがあって、それで深度センサーのデータを表示できるんです。ターミナルを3枚開いて、以下のコマンドを実行してください。

• ターミナル#1

$ roslaunch turtlebot_bringup minimal.launch

• ターミナル#2

$ roslaunch turtlebot_bringup 3dsensor.launch

• ターミナル#3

$ roslaunch turtlebot_rviz_launchers view_model.launch

これで、RVizの画面が表示されるはずです。画面が表示されたら左下の[Add]ボタンを押して、表示されたダイアログの[By display type]タブの[PointCloud2]アイコンを選択し、[OK]ボタンを押してください。RVizの画面左側の[Displays]パネルに[PointCloud2]アイコンが追加されるはずです。なお、ここまでGUI上で実施した作業は、プログラミングでのメッセージの型の指定に相当します。

ROSではメッセージとトピックの両方が必要なのは前の章でやりましたから、メッセージに続いてトピックを設定しましょう。[PointCloud2]アイコンの下の[Topic]ドロップ・ダウン・リストを開いて、「/camera/depth_registered/points」（シミュレーターの場合は「/camera/depth/points」）を選択してください。これで、深度センサーのデータが画面に表示されるはずです。マウスの左ボタンのドラッグで回転、シフト・キーを押しながらの左ドラッグで平行移動、右ドラッグで拡大/縮小できますから、グリグリ回して楽しんでみてください。

グリグリ回している途中のキャプチャなので分かりづらいですけれど、3Dのデータができています。

なお、実機の場合でトピックを「/camera/depth/points」に変更すると、カメラからの画像を合成していない（色が着いていない）点群が表示されます。このように、トピックやメッセージの内容を調べるためにもRVizは使えますから、いろいろと活用してみてください。

PointCloud2

さて、先ほどRVizのメッセージ型として選択したのは、PointCloud2でした。ポイント・クラウドの日本語訳は点群（数学の点群と字は同じだけど意味は違う）で、点の集合です。RVizの[PointCloud2]アイコン以下の[Style]を「Points」、[Size (Pixels)]を「1」にして点群部分を拡大してみると、点の集合であることが分かります。なお、PointCloud2の「2」は、2次元じゃなくてバージョン2の「2」です。以前のPointCloudメッセージは拡張性が不足していたみたいで、新しくPointCloud2が定義されました。

Wikipediaによれば、点群から面を計算してポリゴンにしたり、NURB（Non-uniform rational B-spline）で曲面を出したりすることもできるようです。うん、点群ってなんだか便利そう。ただ表示するだけでも、ちょっと面白いですしね。そうだ、TurtleBotにその場でグルッと回転させて、周囲360°の点群を取得して表示してグリグリしたら楽しいんじゃね？

Subscriber

というわけで、TurtleBotの周囲360°の点群を生成するROSプログラムを作りましょう。そのために、まずは深度センサーからデータをもらわないと。

いつものpackage.xmlとCMakeLists.txt

PointCloud2メッセージを扱うので、いつものとおりに参照するパッケージを追加しなければなりません。でも、PointCloud2のパッケージって、どうやって調べればよいのでしょうか？　答え。rosmsg show PointCloud2で調べられます。

ryo@ryo-T550:~$ rosmsg show PointCloud2
[sensor_msgs/PointCloud2]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

というわけで、sensor_msgsを追加しましょう。

package.xml

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>geometry_msgs</build_depend>
  <build_depend>nodelet</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>  <!-- 追加 -->
  
  <run_depend>geometry_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>
  <run_depend>sensor_msgs</run_depend>      <!-- 追加 -->

  <!-- 略 -->
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nodelet
  roscpp
  sensor_msgs  <!-- 追加 -->
)

# 略

roslaunchで、トピックをリマップ。あと、includeも

先ほどRVizで点群を見た時、TurtleBotの実機とシミュレーターで、設定するトピックが異なっていました。実機は/camera/depth_registered/pointsで、シミュレーターでは/camera/depth/pointsでしたもんね。ということは、トピックだけが異なっていて他は同じプログラムを2つ作らなければならない？

そんな保守性ダダ下がりの馬鹿な話はないわけで、roslaunchに少し記述を追加するだけでトピックをリマップできます。

実機用

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="points" to="camera/depth_registered/points"/>  <!-- 追加 -->
  </node>
</launch>

シミュレーター用

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="points" to="camera/depth/points"/>  <!-- 変更 -->
  </node>
</launch>

あれ？　トピックだけが異なっていて他は同じlaunchファイルを2つ作るのは、保守性ダダ下がりでやっぱり馬鹿らしいので状況は悪いまま？　というわけで、roslaunchのドキュメントにのっている、<arg>と<include>を使ってみます。ついでに、mobile_base/commands/velocityも<remap>しちゃいましょう。

実機用（launch/make_world_models.launch）

<launch>
  <arg name="velocity" default="mobile_base/commands/velocity"/>  <!-- argを宣言 -->
  <arg name="points" default="camera/depth_registered/points"/>   <!-- argを宣言 -->
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="velocity" to="$(arg velocity)"/>  <!-- argを使用 -->
    <remap from="points" to="$(arg points)"/>      <!-- argを使用 -->
  </node>
</launch>

シミュレーター用（launch/make_world_models_in_gazebo.launch）

<launch>
  <include file="$(find six_point_two_eight)/launch/make_world_models.launch">
    <arg name="points" value="camera/depth/points"/>  <!-- argの値を上書きします。 -->
  </include>
</launch>

はい、これで重複がなくなりました。<include>と<arg>って便利ですね。ついでですから、turtlebot_bringup minimal.launchとturtlebot_bringup 3dsensor.launchを2回実行するのは面倒なので、<include>を使用してこの2つをまとめるlaunchファイルも作成しましょう。

TurtleBot起動用（launch/turtlebot_driver.launch）

<launch>
  <include file="$(find turtlebot_bringup)/launch/minimal.launch" />
  <include file="$(find turtlebot_bringup)/launch/3dsensor.launch" />
</launch>

make_world_models.h

以上で準備は完了。プログラミングに入りましょう。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>  // 追加。

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber points_subscriber_;  // サブスクライバーを保持するメンバー変数。
    ros::Timer timer_1_;
    ros::Timer timer_2_;

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);  // remapに合わせて、トピックを変更。

      // PointCloud2を取得するサブスクライバーを設定します。
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,  // トピックはremapのfromの値。
        [&](sensor_msgs::PointCloud2ConstPtr message) {  // ラムダ式の展開のされかた次第では（具体的には、中で外側の変数や関数を呼ばない場合は）、const auto&は使えませんでした。
          ROS_INFO_STREAM("Got PointCloud2. Width: " << message->width);  // とりあえず、データを取得したことを表示。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));  // 前進ではなく、回転に変更。1周（M_PI * 2）を30秒で回る速度を指示します。
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),  // 計算上は30秒でよいはずなのですけど、30秒では一周してくれませんでしたので、多めの値で。
        [&](const auto& message) {
          points_subscriber_.shutdown();  // ros::Subscriberの終了は、stop()ではなくてshutdown()です。
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

subscribeは、ros::Subscriberを使用して実施します。getNodehandle()してros::NodeHandleを取得して、そのメンバー関数のsubscribe<メッセージの方>(トピック名、キューのサイズ、メッセージ取得時の処理)を呼び出してください。引数のトピック名はmake_world_models.launchの<remap>のfrom属性に合わせ、キューのサイズは処理中に次のデータがやってきても取りこぼさないよう、多めに10を指定してみました。

処理は、例によってラムダ式で指定します。C++のラムダ式はその内容によって展開のされ方が違う、かつ、しかもROS側での受け取り引数型のboost::functionは癖が強いみたいで、ラムダ式の引数の型をconst auto&にできる場合とできない場合がありました。どうやら、ラムダ式の中でキャプチャした変数や関数を呼び出しているのでファンクター相当品が生成される場合はconst auto&OKで、今回のコードのようにタダの無名の関数で表現できる場合は駄目みたいです。というわけで、今回はメッセージを受け取る処理の引数の型を明示しました。この型は、コンスタントでスマート・ポインターな型のエイリアスで、メッセージの型の後ろにConstPtrという文字列を付加した型になります（ros::NodeHandle.subscribe()のコードを見るとconst sensor_msgs::PointCloud2&を引数に使うバージョンもあるのですけれど、ラムダ式の場合は駄目でした）。処理の内容は、rosmsg show sensor_msgs/PointCloud2で見ても何を表現しているのか分からないデータばかりだったので、とりあえず、widthを表示するだけにしています。

とまぁ、これでセンサーのデータを読み込めるようになったわけですけど、それにしても、センサーのデータを「読み込む」処理なのに、データを渡された時に実行する処理を渡す形になっているのは、なぜなのでしょうか？　私は、「読み取りなんだから戻り値でいーじゃん」と感じました。

その答えは多分、ROSは様々な周波数で動作する大きめのユニットを統合するための環境だから。「様々な周波数」なのだから、前の章の最初の頃のTimerを使う前のプログラムのようなループを組んで、「一定の周波数」で動作する中でセンサーのデータを読み取る方式は採用できません。センサーやユニットがデータを送りたいタイミングでデータを取得できないとね。だから、処理を登録する形になっているんだと思います。ROS.orgに載っているプログラムはサンプル・プログラムなので単純な場合が多く、単純な処理なのでループで実現していてArduinoのコードと見た目そっくりでだったらArduinoみたいに読み取りでいいじゃんと混乱してしまうのですけれど、実際のアプリケーションを作成すれば、この方式が優れていることが分かると考えます。まぁ、使いづらくて腹がたつ場合も多いんですけど、後述するactionlibとかラムダ式とスマート・ポインターの組み合わせとかで対処できるのでまぁいいかなぁと。

nodelet_topic_tools::NodeletThrottle

とはいえ、データを受け取って処理する側の都合も重要です。publish側の深度センサーが全力で送ってくるデータをすべて処理するのは、かなり大変ですもんね。私が使っているASUS Xtion PRO LIVEの読み取りは30fpsなんで、30秒かけてTurtleBotを一周させる先ほどのプログラムだと、点群の数が900個になっちゃう。なんとかして、深度センサーの周波数を下げたい。

そんな時は、nodelet_topic_toolsパッケージのNodeletThrottleが便利です。メッセージの流量制御を、パフォーマンスを落とさずにとても簡単に実現できます。さっそく使ってみましょう。

package.xmlとCMakeLists.txt

依存するパッケージに、nodelet_topic_toolsを追加してあげてください。これまで何度も見て飽きているでしょうから、コードは省略します。

include/six_point_two_eight/point_cloud_2_throttle.h

nodelet_topic_tools::NodeletThrottleクラスを継承して、throttle()メンバー関数を実装する……などという手間はかかりません。ただし、取り扱うメッセージの型だけは指定しなければなりませんので、usingしてテンプレート引数を補います。C++14（正しくはC++11以降）ではtypedefとusingの両方があって、usingの方が高機能です。nodelet_topic_toolsのドキュメントではtypedefを使っていますけど、そして、今回の使い方ではどっちを使っても変わらないのですけれど、コードを統一するためにusingしましょう。

#pragma once

#include <nodelet_topic_tools/nodelet_throttle.h>
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  using PointCloud2Throttle = nodelet_topic_tools::NodeletThrottle<sensor_msgs::PointCloud2>;
}

src/six_point_two_eight.cpp

Nodeletと同様に、エクスポートします。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/point_cloud_2_throttle.h"  // 追加。

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)  // 追加。

six_point_two_eight.xml

やはりNodeletと同様に、ROSに名前を登録します。

<library path="lib/libsix_point_two_eight">
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>  <!-- 追加 -->
</library>

launch/make_world_models.launch

起動もNodeletと一緒。topic_inから取得したメッセージを、<param name="update_rate">を通じてHzで設定した周波数でtopic_outに流します。

<launch>
  <arg name="velocity" default="mobile_base/commands/velocity"/>
  <arg name="points" default="camera/depth_registered/points"/>
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="velocity" to="$(arg velocity)"/>
    <remap from="points" to="$(arg points)_throttled"/>  <!-- 修正 -->
  </node>
  <!-- ここから追加 -->
  <node pkg="nodelet" type="nodelet" name="point_cloud_2_throttle" args="load six_point_two_eight/point_cloud_2_throttle six_point_two_eight_nodelet_manager" output="screen">
    <remap from="topic_in" to="$(arg points)"/>
    <remap from="topic_out" to="$(arg points)_throttled"/>
    <param name="update_rate" value="0.5"/>  <!-- 1秒に0.5回なので、2秒に1回。 -->
  </node>
</launch>

はい、これで深度センサーからのデータ量を減らすことができました。

PCLを使用して、点群を保存する

とまぁ、いろいろやってPointCloud2を取得できたわけですけれど、取得できただけではなんの意味もありません。でも、rosmsg sensor_msgs/PointCloud2で仕様を見ても、どう使えばよいのかよくわからないし……。

というわけで、PCLと省略して呼ばれるPoint Cloud Libraryを使ってみましょう。

PCLでできること

PCLのドキュメントを開くと、PCLの説明でよく使われる下の絵が表示されます。

filtersモジュールでフィルタ処理して特徴を保ったままで点の数を減らし、featuresモジュールで特徴量を推定して座標位置以外のモノサシを用意して、keypointsモジュールで特徴点を抽出して点群処理の際に注目すべき点を箇所を特定する。registrationモジュールで複数の点群を統合して大きな点群を作成し、kdtreeモジュールやoctreeモジュールで点を構造化する。segmentationモジュールとsample_concensusモジュールで点群を形状に基づいて分割し、surfaceモジュールで面を構築して3次元コンピューター・グラフィックスのライブラリに流す。recognitionモジュールで物体認識して処理対象を見つけてioモジュールで保存したり、visualizationモジュールでスクリーンに表示する……と（点群処理の素人が書いています。間違いがあったらごめんなさい）、とにかく多機能です。

PCLを使う場合の、コードの書き方

プログラマーとしてPCLについて知って置かなければならない最も重要なことは、「C++のライブラリであること」と「バージョン1.7.2ではC++14（C++11も）に対応していないこと」です。

ROSからPCLを扱うための便利機能が詰まったpcl_rosパッケージを使ったとしても、ROSの通信機能経由でPCLの機能を使えるわけではありません。ROSそのものはC++とPythonとCommon Lispとその他多数の言語で使えるのですけれど、PCLを使うプロジェクトではC++一択になります（PCLのPythonのバインディングもありますけれど、カバー範囲が狭い）。

C++14に対応していないというのはC++14の機能を使用して書かれていないという意味ではなく、PCLを使うコードをC++14モードでコンパイルすると、コンパイルはできるけど実行時に異常終了しちゃうということです（異常終了するかどうかは、使う機能次第ですけど。普通に使える機能もありました。PCL1.8でC++11でも大丈夫になるみたいなことは書いてあったのですけど、未確認です）。

なので、PCLを使用する部分は通常のコードとは分離して、しかもC++03で書かなければなりません。以下に、その具体的な方法を示します。とりあえずの処理として、PCLを使用して点群を保存してみました。

package.xmlとCMakeLists.txt

package.xmlとCMakeLists.txtに、pcl_rosパッケージを追加してください。

include/six_point_two_eight/point_cloud_utilities.h

コードの分離を確実にするために、C++14とPCLを完全に排除した*.hを作ります。関数宣言だけを、記述しました。

#ifndef SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H
#define SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H

#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
}

#endif

src/point_cloud_utiliites.cpp

PCLのioモジュールを使えば点群の保存なんて簡単……ではありますけど、悲しいことにそれなりの量のコードを書かなければなりません。PCLって機能はスゴイんですけど、プログラミング・スタイルが少し独特なんですよ。入力も出力も引数でやる方式なので、関数呼び出しのためだけに変数を定義したりreturnを別に書いたりしなければなりません。引数をスマート・ポインターで渡す時と参照で渡すときがあって、混乱しちゃいますし。

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "six_point_two_eight/point_cloud_utilities.h"

// 点群の型の整理。
typedef pcl::PointXYZRGB Point;  // 本稿では、色付きの点を使用します。あと、C++03なので、usingではなくtypedefを使用します。
typedef pcl::PointCloud<Point> PointCloud;

// 引数で指定した点群のファイル・パスを取得します。パスは、/tmp/six_point_two_eight_<点群の時刻>.pcdになります。
std::string filePathString(PointCloud::ConstPtr point_cloud) {
  std::stringstream file_path_stringstream;
  file_path_stringstream << "/tmp/six_point_two_eight_" << std::setfill('0') << std::setw(19) << pcl_conversions::fromPCL(point_cloud->header).stamp.toNSec() << ".pcd";

  return file_path_stringstream.str();
}

// 点群を保存します。
PointCloud::Ptr savePointCloudFile(PointCloud::ConstPtr point_cloud) {
  std::string file_path_string = filePathString(point_cloud);  // C++03なので、autoは使えません。

  pcl::io::savePCDFileBinary(file_path_string, *point_cloud);
  ROS_INFO_STREAM("PointCloud is saved to " << file_path_string);

  //  続けて別の関数を呼び出せるように、点群を返します。
  return PointCloud::Ptr(new PointCloud(*point_cloud));  // boost::shared_ptr<const X>からboost::shared_ptr<X>には変換できないので、新たにインスタンスを生成します。
}

// ROSの点群を、PCLの点群に変換します。
PointCloud::Ptr fromROSMsg(sensor_msgs::PointCloud2ConstPtr points) {
  PointCloud::Ptr converted_point_cloud(new PointCloud());
  pcl::fromROSMsg(*points, *converted_point_cloud);
    
  return converted_point_cloud;
}

// PCLの点群を、ROSの点群に変換します。
sensor_msgs::PointCloud2Ptr toROSMsg(PointCloud::ConstPtr point_cloud) {
  sensor_msgs::PointCloud2Ptr converted_points(new sensor_msgs::PointCloud2());
  pcl::toROSMsg(*point_cloud, *converted_points);
  
  return converted_points;
}

// 機能へのインターフェースを提供します。
sensor_msgs::PointCloud2Ptr six_point_two_eight::savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points) {
  return toROSMsg(savePointCloudFile(fromROSMsg(points)));
}

include/six_point_two_eight/make_world_models.h

ロボット制御側の（C++14の）コードは以下の通り。point_cloud_utilitiesがPCLを隠蔽してくれていますから、こちらは簡単です。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/point_cloud_utilities.h"  // 追加。

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber points_subscriber_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&](sensor_msgs::PointCloud2ConstPtr message) {
          savePointCloud2File(message);  // 点群を保存します。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),
        [&](const auto& message) {
          points_subscriber_.shutdown();
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

CMakeLists.txt

ビルドを制御するために、CMakeLists.txtも変更しなければなりません。今回のように特定のファイルにだけコンパイル・オプションを設定したい場合は、set_cource_files_properties()を使用できます。以下のように、PCLに対応するために「-std=c++03 -Wno-deprecated」を設定してください。-std=c++03に加えて-Wno-deprecatedを指定しているのは、PCLのvisualizationモジュールが使用しているパッケージが使用を推奨されていない機能を使っているためです。ビルドのたびに警告が表示されて、しかも我々のコード以外が原因で対応不可能では、ストレスがたまるだけですもんね。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nodelet
  nodelet_topic_tools
  pcl_ros
  roscpp
  sensor_msgs
)

# 略

catkin_package(
#  CATKIN_DEPENDS
)

add_compile_options(
  "-std=c++14"
)

# PCL1.7.2はC++14に対応していません。あと、visualizationがdeprecatedな機能を使用しています。コンパイル・オプションを指定して対応します。
set_source_files_properties(src/point_cloud_utilities.cpp  # 追加
  PROPERTIES COMPILE_FLAGS "-std=c++03 -Wno-deprecated"    # 追加
)                                                          # 追加

# 略

実行……してみたら、まだまだ機能が足りない

保存した点群は、pcl_viewerというコマンドで見ることができます。プログラムを実行し、Ctrl-cで止め、pcl_viewerで見てみましょう。

• ターミナル#1

$ roslaunch six_point_two_eight turtlebot_driver.launch

• ターミナル#2

$ catkin_make && rm -f /tmp/six_point_two_eight_*.pcd && roslaunch six_point_two_eight make_world_models.launch
# 処理が完了したら、Ctrl-cでプログラムを止める。

$ pcl_viewer /tmp/*.pcd
# 対象が画面に映るよう、rキーを押してカメラ・アングルをリセットする。
# 使いたは、hキーを押すとターミナルに表示される。
# マウスでグリグリしてみる。
# 飽きたら、qキーを押して終了させる。

ターミナル#2の1行目。コマンドを&&でつなぐと、「前のコマンドが成功したら後ろのコマンドを実行する」という指示になります。ビルドして、前の実行で生成された点群のファイルを消して、make_world_modelsを実行する（ただし、途中で失敗したら処理を止める）の意味になるわけ。この形で起動することで、ビルド忘れや過去の実行結果の混入を避けています。

で、その肝心のpcl_viewerで見た実行結果は、なんだか意味不明です。

どうしてこうなってしまったかというと、深度センサーからの相対座標の点群を、同じ座標系に重ねたから。以下のように1枚だけを表示するなら、普通に点群を見られます。

$ pcl_viewer `ls -s /tmp/six_point_two_eight_*.pcd | head -n 1`

というわけで、TurtleBotの姿勢に合わせて、適切に点群を移動/回転させてあげなければならないことが分かりました。

オドメトリー

少し話は変わりますけど、車やバイクにはオドメーターというのがあって、累計走行距離が分かるようになっています。あと、スピード・メーターで速度が表示できていて、速度は移動距離÷時間で、で、この距離ってどうやって測っているの？

車やバイクによっていくつかバリエーションはあるのですけれど、トランスミッション等の回転速度からタイヤの回転数を推測し、カタログ上のタイヤの外周のサイズをかけて移動距離を計算しています。同じことはロボットでも可能で、モーター等のセンサーから、タイヤが何回転したかが分かります。タイヤのサイズも設計書から分かる。だから、どれだけ移動したかが計算できるはず。TurtleBotのように左右にタイヤが付いている移動方式なら、左右のタイヤの回転数の差から、現在どちらを向いているのかも計算できるはず。

このように、モーター等のセンサーを使用して推定した位置情報を、オドメトリーと呼びます（車のオドメーターとは違って、位置と向きが分かる）。TurtleBotの場合は、/odomトピックでオドメトリーを取得できます。

さっそく、試してみましょう。

• ターミナル#1

$ roslaunch six_point_two_eight turtlebot_driver.launch

• ターミナル#2

$ roslaunch turtlebot_teleop keyboard_teleop.launch

• ターミナル#3

$ rostopic echo /odom
header: 
  seq: 4443
  stamp: 
    secs: 1457333175
    nsecs: 603418136
  frame_id: odom
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 0.20837154614
      y: -0.108868545114
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.305446027598
      w: 0.952209390956
  covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0,
 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
 0.0, 0.0, 0.0]
---

ターミナル#2で適当にキーを押してTurtleBotを移動させると、ターミナル#3のpose.poseのpositionとorientationの値が変わっていきます。ROSの座標系はX軸が前でY軸が左でZ軸が上、回転は上から（Z軸のプラス側からマイナス側を）見て反時計回りで、/odomは起動時の状態からの相対座標/回転となっていることが分かります。

PCLを使用して、座標変換してフィルタリングしてダウンサンプリングする

でも、よっしゃー/odomトピックからオドメトリーを取得して点群を移動/回転させてやるぜーってのは、ごめんなさい、あまりうまくいきません。以下、その理由と対策をやって、ついでなので点群のダウンサンプリングとフィルタリングをやります。

TF

ROSのメッセージは大抵headerを含んでいて、headerのstampはそのメッセージが採取された時刻です。この値が異なっている場合の処理、たとえば5秒前に採取した点群を3秒前のオドメトリーで変換するような処理は、無意味です。だって、その間に移動したかもしれないんですから。

でも、もし7秒前のオドメトリーも知っているならどうでしょうか？　等速度運動をしていると仮定すれば、5秒前の位置は、7秒前の位置と3秒前の位置の中間だと推定することができます。ただし、そのためには過去の状態を複数保持し、間を補完する計算処理が必要です。メモリを食いつぶすわけにはいきませんから、ある程度古くなったメッセージを捨てる処理も必要です。でも、これを/odomトピックのサブスクライバーで作るのは大変そうです。深度センサーがロボット・アームの先に付いているような場合は、さらに面倒です。肩のサーボ・モーターがこの角度で肘のサーボ・モーターがあの角度だから、深度センサーはこの位置でこんな向きのはずだという計算も、とてもとても面倒くさそうです。

だから、ROSのパッケージのTFを使いましょう。TFは、上に挙げた面倒な計算処理をすべて実施してくれます。

TFの解説と使い方は、TF,TF2　完全理解にわかりやすくまとまっていて、すべてのROSプログラマーはこのスライドは絶対に見ておくべきなんですけれど、本稿の範囲だとpcl_rosパッケージのPCLとTFを取り持ってくれる機能を使えば大丈夫かも。この機能を使って、さくっとプログラミングしちゃいましょう。

package.xmlとCMakeLists.txt

package.xmlとCMakeLists.txtに、tfパッケージとtf_conversionsパッケージを追加してください。

include/six_point_two_eight/point_cloud_utilities.h

ごめんなさい。関数宣言の段階でTFの知識が必要だった……。まずは、TFのフレームの話をさせてください。というのも、座標変換前の点群を調べてみると、深度センサーの位置を原点にして、前がZ軸で右がX軸、下がY軸になっているんですよ。

これはROSの座標系とは反転が必要なレベル（いわゆる右手系と左手系のレベル）で異なっているわけで、オドメトリーがどうのこうのの前に、まずはROSの座標系に変換しなければなりません。幸いなことにこの変換は、四元数と呼ばれる複素数を拡張した数体系を使えば、簡単に変換できるみたいです（x = -0.5、y = 0.5、z = -0.5、w = 0.5の四元数で変換できた）。で、次に問題になるのはTurtleBotの原点と深度センサーの原点が異なっていること。これは、ベクトルの引き算で変換できます。でも、こんな変換をいちいちプログラムしていくのは大変すぎます。なので、ロボットの構成要素と構成要素を、必要な座標系の変換でつなぐことにしましょう。camera_rgb_optical_frame→（変換）→camera_rgb_frame→（変換）→base_linkみたいな感じ。この方式ならプログラミングではなくてデータ作成で済むので、とても楽になります。

ただ、base_linkには深度センサー以外の構成要素もつながっていて、例えばwheel_right_link（右の車輪）なんてのもあります。そう考えると、構成要素の関係はリストではなくて木構造が適切かなぁと。で、さらに考えてみると、オドメトリーとロボット本体の関係も、この木構造で表現できます。変換が毎回同じか、移動によって変わっていくかの違いだけですもんね。というわけで、TFではこの構成要素をフレームと呼んで木構造にして、座標変換でつないでいきます。TureltBotのドライバーを動かした状態だと、odomフレーム（名前は一緒ですけど、odomトピックとは別物）が根で、camera_rgb_optical_frameフレームやwheel_right_linkフレームを葉に持つ木構造が作られています。

TFで座標変換する際には、このフレームを使用します。どのフレームの座標系からどのフレームの座標系に変換するかを指定するわけ。指定するフレームは木のどこにあっても大丈夫です。木構造を辿ればよいわけですから。今回の場合、点群はcamera_rgb_optical_frameの座標系であることが分かっている（というか、ROSではheader.frame_idにメッセージのフレームが設定されている）ので、変換先のフレームだけ指定すればよいことになります。なので、二番目の引数としてtarget_frameを渡しています。

// 略

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(  // 追加。
    sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust = 0.0);  // 追加。
}

// 略

残りの引数のx_adjustは、プログラムを作成して試してみたら奥行きに少しずれがったから追加してみました。こんな引数があってはならないのですけれど、ここだけはご容赦ください。

src/point_cloud_utilities.cpp

なんだかとても長いコードになってしまいました……。その理由は、1つのグローバル変数と3つの処理が必要だったから。以下、それぞれについて述べます。

グローバル変数。transform_listener_のところ。TFが機能するには、流れてくる姿勢情報（親フレームからどのように座標変換したか）を蓄えておかなければなりません（そうしないと指定時刻の姿勢を推定できなくなってしまう）。その役割を担っているのがtf::TransformListenerです。常に待機しておかなければならないわけで、だから一般にグローバル変数にします。気持ち悪いけど。

処理その1。奥行きの補正です。補正しないバージョンのプログラムでデータを調べてみたら、なんだか少しだけずれていたので追加しました。C++14なら拡張forが使えるのですけど、C++03なのでBOOST_FOREACHを使用しました。

処理その2。TFは5秒前の姿勢情報と3秒前の姿勢情報から、4秒前の姿勢を推測できます。しかし、3秒前の姿勢情報の後の姿勢情報を受け取っていない場合は、2秒前の姿勢を推測できません。TF関連でよくある失敗として、「現在の姿勢を教えてくれ」と聞いて毎回もれなくエラーになるというのがあります。TFはデータの間を線形で補間する機能しかなくて、未来のデータなんて存在しないのだからまぁ当たり前なんですけどね。本稿のコードでは、点群の時刻が新しすぎて、点群の時刻以降のロボットの姿勢情報を受け取れていないのでエラーになるという危険性があります。それでは困るので、点群の時刻よりもあとの姿勢情報を受信するまで待つように指示しています。これがwaitForTransform()メンバー関数を呼び出しているところ。で、「待っても姿勢情報が来なかった」場合と、「姿勢情報を貯めこむTransformListenerに最初の姿勢情報が届く前の点群がとれちゃったので待ってもどうにもならない」場合は、エラーになります。だから、if文で戻り値をチェックしてエラー・ハンドリングしました。

処理その3。やっと本番の座標変換。ここでは普通は失敗しないのですけど、コードに統一性を持たせるためにエラー・ハンドリングしてみました。

#include <boost/foreach.hpp>  // 追加。
#include <pcl/filters/filter.h>  // 追加。
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>  // 追加。
#include <tf/transform_listener.h>  // 追加。

#include "six_point_two_eight/point_cloud_utilities.h"

typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud;

static tf::TransformListener transform_listener_;  // 追加。

// 略

// 点群をtarget_frameの座標系に変換します。その際に、x_adjustで指定された値で、奥行きも補正します。
PointCloud::Ptr transformPointCloud(
  PointCloud::ConstPtr point_cloud, const std::string& target_frame, double x_adjust) 
{
  // 奥行きがずれているようだったので、補正します。
  PointCloud::Ptr x_adjusted_point_cloud(new PointCloud(*point_cloud));
  BOOST_FOREACH(Point& point, *x_adjusted_point_cloud) {
    point.z += x_adjust;  // 深度センサーの生データは、実は座標系が異なります。右がX軸で下がY軸、前がZ軸なんです。
  }

  // TFは姿勢情報と姿勢情報の間しか補完できない（最新の姿勢情報よりも後の時刻で問い合わせるとエラーになる）ので、点群よりも後の姿勢情報が来るまで待ちます。
  std_msgs::Header header = pcl_conversions::fromPCL(point_cloud->header);
  if (!transform_listener_.waitForTransform(target_frame, header.frame_id, header.stamp, ros::Duration(1.0))) {
    ROS_WARN_STREAM("Can't wait...");
    throw std::exception();
  }
  
  // 内部でTFを使用して、座標変換します。
  PointCloud::Ptr transformed_point_cloud(new PointCloud());
  if (!pcl_ros::transformPointCloud(
    target_frame, *x_adjusted_point_cloud, *transformed_point_cloud, transform_listener_)) 
  {
    ROS_WARN("Transform failed...");
    throw std::exception();
  }

  // 座標変換した点群を返します。
  return transformed_point_cloud;
}

// 略

sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(
  sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust) 
{
  return toROSMsg(transformPointCloud(fromROSMsg(points), target_frame, x_adjust));
}

include/six_point_two_eight/make_world_models.h

C++14のロボット制御側は、いつものとおりに簡単です。point_cloud_utilities.cppのエラー・ハンドリングで例外を使用しましたので、try/catchを追加しました。

// 略

namespace six_point_two_eight {
  // 略
  
  class MakeWorldModels : public nodelet::Nodelet {
    // 略
    
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&](sensor_msgs::PointCloud2ConstPtr message) {
          try {                                 // 追加。
            savePointCloud2File(
              transformPointCloud2(             // 追加。
                message,
                "odom",                         // 追加。
                -0.05));                        // 追加。
            
          } catch (const std::exception& ex) {  // 追加。
            ROS_WARN_STREAM(ex.what());         // 追加。
          }                                     // 追加。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),
        [&](const auto& message) {
          points_subscriber_.shutdown();
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

試してみた

作成したプログラムを、動かしてみました。結果は以下の通り。正しく座標変換できているけど、まだダメそう。

だって、遠くにある点の精度が悪すぎますもんね。センサーの有効範囲外なので、遠くにある点の精度向上は望めません。近くの点だけをフィルタリングする処理が必要みたいです。

フィルタリング

C++14なら、boostと組み合わせてラムダ式を引数に与えて1行で簡単にフィルタリングできる……のですけど、C++03で書いている今は駄目。PCLに点群の点をフィルタリングする機能がありますので、それで我慢しましょう。

PCLのフィルタリングのチュートリアルの中の、Removing outliers using a Conditional or RadiusOutlier removalを読むと、フィールドと定義済み演算子と比較対象の値を渡すという簡単なフィルターが使われています。このやり方だと直方体の切り抜きしかできなくて、直方体だとセンサーからの距離が一定にならないので駄目。他によいのがあるかもしれないとリファレンスを見ても、うん、見つかりませんね……。というわけで、pcl::ConditionRemovalクラスと独自作成のpcl::Conditionクラスで実装しました。

include/six_point_two_eight/point_cloud_utilities.h

センサーからの距離が一定の値以下の点を集めたいわけですから、球の中にある点だけを抽出すればよい。球を表現するには中心と半径が必要。というわけで、点群と中心と半径を引数に撮るgetShperePointCloud()を宣言しました。

// 略

#include <geometry_msgs/Point.h>  // 追加。
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(  // 追加。
    sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius);  // 追加。
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(
    sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

// 略

src/point_cloud_utilities.h

フィルタリング条件は、pcl::ConditionBaseを継承して作成するみたいです。ラムダ式なんて贅沢は言わないけど、せめてファンクターにして欲しかった……。getSpherePointCloud()関数は、作成したクラスを使うだけです。

#include <boost/foreach.hpp>
#include <pcl/filters/conditional_removal.h>  // 追加。#include <pcl/filters/filter.h>は削除。
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

// 略

// 目的に合致するpcl::Conditionがなかったので、新規に作成しました。
template <typename PointT>
class SphereCondition : public pcl::ConditionBase<PointT> {
private:
  Eigen::Vector3f center_;
  double radius_;

public:
  SphereCondition(const Eigen::Vector3f& center, double radius)
    : center_(center), radius_(radius)
  {
    ;
  }

  virtual bool evaluate(const PointT& point) const {
    // 中心からの距離がradius_以下ならtrueを返す。
    return (point.getVector3fMap() - center_).norm() <= radius_;  // Eigenでは、ベクトル演算ができます。norm()は、原点からの距離を返します。
  }
};

// pcl::ConditionalRemovalを使用して、球の内側の点のみを抽出します。
PointCloud::Ptr getSpherePointCloud(PointCloud::ConstPtr point_cloud, const Eigen::Vector3f& center, double radius) {
  // コンディションを生成。
  SphereCondition<Point>::Ptr(new SphereCondition<Point>(center, radius));

  // pcl::ConditionRemovalを生成。
  pcl::ConditionalRemoval<Point> conditional_removal(condition);

  // 入力用の点群を設定。
  conditional_removal.setInputCloud(point_cloud);
  
  // 出力用の点群を設定。
  PointCloud::Ptr sphere_point_cloud(new PointCloud());
  conditional_removal.filter(*sphere_point_cloud);

  // 球の内側の点のみの点群をリターン。
  return sphere_point_cloud;
}

// 略

sensor_msgs::PointCloud2Ptr six_point_two_eight::getSpherePointCloud2(
  sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius) 
{
  return toROSMsg(getSpherePointCloud(fromROSMsg(points), Eigen::Vector3f(center.x, center.y, center.z), radius));
}

サブスクライバーとラムダ式とスマート・ポインター

作成したgetSpherePointCloud2()関数を呼びだすには点群とオドメトリーの両方のデータが必要で、その2つは異なるラムダ式の引数です。さて、どうしましょうか？

ROSのサンプル・プログラムでは、メンバー変数に格納することで対応しているようです。以下みたいな感じ。

class Foo : public nodelet::Nodelet {
private:
  ros::Subscriber odom_subscriber_;
  ros::Subscriber points_subscriber_;

  // オドメトリーを共有するためのメンバー変数。
  nav_msgs::Odometry odom_;
  
  void odomCallback(nav_msgs::OdometryConstPtr message) {
    odom_ = *message;
  }
  
  void pointsCallback(sensor_msgs::PointCloud2ConstPtr message) {
    // messageとodom_を使う処理。
  }

public:
  void onInit() {
    odom_subscriber_   = getNodeHandle().subscribe("odom",   1, &Foo::odomCallback,   this);
    points_subscriber_ = getNodeHandle().subscribe("points", 1, &Foo::pointsCallback, this);
  }
};

でも、この方式って、状態遷移があると条件分岐が増えてしまうので、できれば避けたい。例えば、オドメトリーを使用して目標地点まで移動したあとに深度センサーのデータを取得だと、以下のようにif文だらけになってしまいます。

class Foo : public nodelet::Nodelet {
private:
  ros::Subscriber odom_subscriber_;
  ros::Subscriber points_subscriber_;

  // オドメトリーを共有するためのメンバー変数。
  nav_msgs::Odometry odom_;
  
  // 移動先を示すメンバー変数。
  geometry_msgs::Pose goal_pose_;

  // 移動が完了したかどうかを示すメンバー変数。
  bool is_reached_goal_;
  
  void odomCallback(nav_msgs::OdometryConstPtr message) {
    if (!is_reached_goal_) {
      if (!isReached(message, goal_pose_)) {
        // goal_pose_に近づくように、ロボットを移動させる。
        
      } else {
        is_reached_goal_ = true;
        odom_ = *message;
      }
    }
  }
  
  void pointsCallback(sensor_msgs::PointCloud2ConstPtr message) {
    if (is_reached_goal_) {
      // messageとodom_を使う処理。
    }
  }

public:
  void onInit() {
    odom_subscriber_   = getNodeHandle().subscribe("odom",   1, &Foo::odomCallback,   this);
    points_subscriber_ = getNodeHandle().subscribe("points", 1, &Foo::pointsCallback, this);
    
    goal_pose_ = // 目標地点。
    is_reached_goal_ = false;
  }
};

この種の問題は、デザイン・パターンのStateパターンで解決できます。でも、Stateパターンって、クラスがいっぱい必要で書くのが面倒なんですよ……。だから、C++14な我々はラムダ式で解決しましょう。

class Foo : public nodelet::Nodelet {
private:
  ros::Subscriber odom_subscriber_;
  ros::Subscriber points_subscriber_;

  void takePhoto(const nav_msgs::Odometry& odom) {
    points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
      "points", 1,
      [&, odom](sensor_msgs::PointCloud2ConstPtr message) {
        // messageとodomを使う処理。
      });
  }

  void moveTo(const geometry_msgs::Pose& goal_pose) {
    odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
      "odom", 1,
      [&, goal_pose](nav_msgs::OdometryConstPtr message) {
        if (!isReached(message, goal_pose)) {
          // goal_poseに近づくように、ロボットを移動させる。
          
        } else {
          this->takePhoto(*message);
          odom_subscriber_.shutdown();
        }
      });
  }

public:
  void onInit() {
    moveTo(/* 目標地点 */);
  }
};

ほら、簡単になったでしょ？

ついでですから、一番最初に書いたオドメトリーを共有する場合のコードもラムダ式版にしてみましょう。C++はラムダ式が参照していても関数を抜けると自動変数を破棄してしまうので、スマート・ポインターを使って寿命を管理しています。

class Foo : public nodelet::Nodelet {
private:
  ros::Subscriber odom_subscriber_;
  ros::Subscriber points_subscriber_;

public:
  void onInit() {
    // 共有したい情報を格納するstruct。
    struct State {
      nav_msgs::Odometry odom;
    };
    
    // 寿命管理のために、スマート・ポインターを使用します。
    std::shared_ptr<State> state(new State());
  
    odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
      "odom", 1,
      [&, state](nav_msgs::OdometryConstPtr message) {
        state->odom = *message;
      });
      
    points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
      "points", 1,
      [&, state](sensor_msgs::PointCloud2ConstPtr message) {
        // messageとstate->odomを使う処理。
      });
  }
};

メンバー変数版とは違い、状態が増えてもメンバー関数を増やすことで対応できます。Sateパターンにおけるクラス相当のモノを、ラムダ式なら関数で作れるんです。

というわけで、方針が決まりましたから、プログラミングに入りましょう。

package.xmlとCMakeLists.txt

nav_msgs/Odometryメッセージを使いますので、package.xmlとCMakeLists.txtに、nav_msgsパッケージを追加してください。

include/six_point_two_eight/make_world_models.h

ロボット制御側のコードはこんな感じ。今作成している処理の場合はすべてをodomのサブスクライバーのラムダ式の中に書く方式が適切なのですけれど、この後に本稿で作る処理にはスマート・ポインターでステート共有できる処理はなさそうで、それだとスマート・ポインター方式を使うサンプルがどこにもなくなってしまう……。だから、敢えてスマート・ポインター方式にしてみました。あと、深度センサーのカメラは起動直後は安定しないみたいなので、3秒待つ処理も付け加えてみました。ラムダ式を使うと、いろいろ簡単で捗ります。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>  // 追加。
#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/point_cloud_utilities.h"

namespace six_point_two_eight {
  // 略。

  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber odom_subscriber_;  // 追加。
    ros::Subscriber points_subscriber_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      // ラムダ式間で共有するステート。C++のラムダ式は寿命管理をしないので、スマート・ポインターで寿命管理します。
      struct State {
        geometry_msgs::Point position;
      };
      std::shared_ptr<State> state(new State());

      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&, state](const auto& message) {
          state->position = message->pose.pose.position;  // 現在位置をステートに保存します。
        });

      auto starting_time = ros::Time::now();  // 開始時刻。
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&, state, starting_time](const auto& message) {  // キャプチャーにstateとstarting_timeを追加。
          // 起動直後は動作が安定しないので、3秒間は何もしません。
          if (message->header.stamp < starting_time + ros::Duration(3.0)) {
            return;
          }
          
          try {
            savePointCloud2File(
              getSpherePointCloud2(  // 追加。
                transformPointCloud2(
                  message,
                  "odom",
                  -0.05),
                state->position,     // TurtleBotの位置から
                1.5));               // 1.5m以内の点群を取得します。
            
          } catch (const std::exception& ex) {
            ROS_WARN_STREAM(ex.what());
          }
        });

      // 略
    }
  };
}

試してみた

試してみると、以下のキャプチャのような感じ。うまくフィルタリングできています。でも、点の密度が濃すぎてちょっと重たいかなと感じます。

PCLにはダウンサンプリングの機能がありますから、さくっとダウンサンプリングして点の数を減らしてみましょう。

ダウンサンプリング

PCLにはいろいろなダウンサンプリングの機能がありますけど、今回使用するのはpcl::VoxelGridクラスです。指定した密度になるように点を間引いてくれます。

include/six_point_two_eight/point_cloud_utilities.h

pcl::VoxelGridクラスは「leaf_sizeで指定した空間あたり点は1つ」にするものなので、2番目の引数としてleaf_sizeを追加しました。

// 略

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size);  // 追加。
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius);
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

// 略

src/point_cloud_utilities.cpp

PCLの少し面倒なコーディング・スタイルにも慣れてきました。ダウンサンプリングの機能を一から作るのに比べれば圧倒的に楽なのですから、このくらいのコードは余裕です。

#include <boost/foreach.hpp>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/voxel_grid.h>  // 追加。
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

// 略

// pcl::VoxelGridを使用して、ダウンサンプリングします。
PointCloud::Ptr downsamplePointCloud(PointCloud::ConstPtr point_cloud, double leaf_size) {
  pcl::VoxelGrid<Point> voxel_grid;
  voxel_grid.setInputCloud(point_cloud);
  voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);

  PointCloud::Ptr downsampled_point_cloud(new PointCloud());
  voxel_grid.filter(*downsampled_point_cloud);

  return downsampled_point_cloud;
}

// 略

sensor_msgs::PointCloud2Ptr six_point_two_eight::downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size) {
  return toROSMsg(downsamplePointCloud(fromROSMsg(points), leaf_size));
}

// 略

include/six_point_two_eight/make_world_models.h

ロボットを制御する側は、例によって簡単です。ダウンサンプリングした後に他の処理を実行したほうが点の数が少なくて効率がよいですから、一番内側で呼び出しています。

// 略

namespace six_point_two_eight {
  // 略
  
  class MakeWorldModels : public nodelet::Nodelet {
  // 略

  public:
    void onInit() {
      // 略
      
      auto starting_time = ros::Time::now();
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&, state, starting_time](const auto& message) {
          if (message->header.stamp < starting_time + ros::Duration(3.0)) {
            return;
          }
          
          try {
            savePointCloud2File(
              getSpherePointCloud2(
                transformPointCloud2(
                  downsamplePointCloud2(  // 追加。
                    message,
                    0.005),               // 0.005m（5mm）あたり、点が1つ。
                  "odom",
                  -0.05),
                state->position,
                1.5));
            
          } catch (const std::exception& ex) {
            ROS_WARN_STREAM(ex.what());
          }
        });

      // 略
    }
  };
}

完成！

ついにプログラム完成です。点の数が減りました。ちなみに、左側が机のキャビネットで、右側が椅子です。

はい、お疲れさまでした。実は作成した点群を拡大するとズレがあって本章の最初に載せたキャプチャのレベルに達していないのですけど、今は気がつかなかったことにして⁸、対象物の周囲360°からの点群の作成に入りましょう。

対象物の周囲360°からの3D点群を作成する

もっとロボットをダイナミックに動かしたい。だいたい、その場でグルッと回る方式だと、ドーナッツみたいなデータになってしまうのでちょっと寂しい。というわけで、対象物の周囲を回りながら深度センサーで点群を採取して、3Dモデルを作るプログラムをやりましょう。以下の画像のような感じ。私が通勤で使っている、サラリーマンとは思えないカバンの3D点群です。

とりあえず、目的地まで移動させてみる

千里の道も一歩から。とりあえず、狙った場所まで移動するプログラムを書いてみましょう⁹。

共有する関数はどうしよう？

移動するためにはgeometry_msgs/Twistメッセージを作成しなければならなくて、作成のためのcreateTwistMsg()関数はmake_world_models.hに書きました。このままだと、同じ関数を2回プログラミングすることになってしまいます。なので、別ファイルに移しましょう。include/six_point_two_eight/utilities.hを作成し、createTwistMsg()関数を移しました。

include/six_point_two_eight/utilities.h

単純にコピー＆ペーストしただけです。

#pragma once

#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
}

include/six_point_two_eight/make_world_models.h

ただ削除して、#includeを調節しただけです。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/utilities.h"
#include "six_point_two_eight/point_cloud_utilities.h"

namespace six_point_two_eight {
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber odom_subscriber_;
    ros::Subscriber points_subscriber_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;

    // 略

9時の方向に0.5m進んでみる

移動プログラムでは、/odomトピックの値に合わせた、適切なgeometry_msgs/Twistメッセージを作成していけばよいはず。目的地までの距離が遠かったり角度のズレが大きい場合は少し速く動いたり回転したりして、近くなったらゆっくりで。目的地と/odomが一致したら、移動終了。ただし、一致と言ってもピッタリにはできないだろうから、少し誤差を認めよう。目的地は、三角関数で計算できるはずだな。

と、この程度を考えたところで、プログラムを組んでみました。先に結果を述べておくと、プログラムが複雑になってしまうので、別のやり方を考えないと駄目な感じでした。

include/six_point_two_eight/utilities.h

位置を表現するためのgeometry_msgs/Pointメッセージ作成関数と、角度を正規化する関数を追加しました。

#pragma once

#include <cmath>

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    // 略
  }

  // geometry_msgs::Pointを作成します。
  inline auto createPointMsg(double x, double y) {
    geometry_msgs::Point msg;
    msg.x = x;
    msg.y = y;

    return msg;
  }

  // -π〜πになるように、角度を正規化します。
  inline auto normalizeAngle(double angle) {
    return std::atan2(std::sin(angle), std::cos(angle));  // -1000とかの場合に備えるための再帰やループは面倒だったので、ライブラリ任せで実現しました。
  }
}

include/six_point_two_eight/make_target_model.h

新規に作成しました。速度調節用のtunedLinearX()メンバー関数とtunedAngularZ()メンバー関数、移動をするためのmoveTo()メンバー関数を2つ（片方は作業用）、あとはいつものonInit()メンバー関数を作成しました。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>

#include <nav_msgs/Odometry.h>

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber odom_subscriber_;

    // 適切な前進速度を返します。
    auto tunedLinearX(double distance, double angle) const {
      if (distance < 0.05) {  // 目的地への距離が0.05mになったら、前進をやめる。
        return 0.0;
      }
      if (std::fabs(angle) > M_PI / 180 * 45) {  // 目的地への角度が±45°を超える場合は、前進しない（で回転する）。
        return 0.0;
      }
      
      return std::min(std::fabs(distance) * 0.5, 0.1);  // 0.1m/secか距離*0.5/secの小さい方を返す。
    }
    
    // 適切な回転速度を返します。
    auto tunedAngularZ(double angle) const {
      if (std::fabs(angle) < M_PI / 180 * 1) {  // 目的地への角度が±1°になったら、回転をやめる。
        return 0.0;
      }
      
      return std::min(std::fabs(angle) * 0.5, M_PI / 180 * 10) * (angle / std::fabs(angle));  // 10°/secか角度*0.5/secの小さい方を返す。マイナスの場合があることに注意。
    }

    // 目的地まで移動します（作業用）。
    auto moveTo(const geometry_msgs::Point& goal_position, nav_msgs::OdometryConstPtr odometry) {
      // 目的地と現在位置の差を計算します。
      auto difference = createPointMsg(
        goal_position.x - odometry->pose.pose.position.x, goal_position.y - odometry->pose.pose.position.y);

      // 距離と角度を計算します。計算が単純なので、TFやEigenを使わずに自前でやります。
      auto distance = std::sqrt(std::pow(difference.x, 2) + std::pow(difference.y, 2));
      auto angle = 
        normalizeAngle(std::atan2(difference.y, difference.x) - tf::getYaw(odometry->pose.pose.orientation));

      // 移動速度を計算します。
      auto linear_x = this->tunedLinearX(distance, angle);
      auto angular_z = this->tunedAngularZ(angle);

      // ロボットを移動させます。
      velocity_publisher_.publish(createTwistMsg(linear_x, angular_z));

      // 移動したかどうかを返します。
      return linear_x != 0.0 || angular_z != 0.0;
    }

    // 目的地まで移動します。
    auto moveTo(geometry_msgs::Point goal_position) {
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&, goal_position](const auto& message) {
          if (this->moveTo(goal_position, message)) {  // 移動した場合は、それで今回分の処理は終了なので、
            return;                                    // そのままリターンします。
          }
          
          ROS_INFO_STREAM("Reached!");  // 移動しない＝到着なので、ログを出して、
          odom_subscriber_.shutdown();  // サブスクライバーをシャットダウンします。
        });
    }
    
  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          auto position = message->pose.pose.position;
          auto yaw = tf::getYaw(message->pose.pose.orientation);  // TFの関数を使用して、ヨーを取得します。

          this->moveTo(
            createPointMsg(
              position.x + std::cos(yaw + M_PI / 2) * 0.5, position.y + std::sin(yaw + M_PI / 2) * 0.5));  // 9時の方向に0.5m移動。
        });
    }
  };
}

うーん、対象物の周囲360°からの点群を作るということは、今回作成した移動の後に対象物が正面に来るように回転して、回転が終わったら深度センサーから点群をとって、また移動に戻るのを繰り返さなければなりません。関数の中から関数を呼び出して延々と処理がつながっていくわけで、それっていわゆるJavaScriptのコールバック地獄じゃん……。なんとかしないと……。

src/six_point_two_eight.cpp

でも、なんとかする前に、まずはプログラムを動かしてテストしないとね。そのためにはコンパイルしなければなりませんので、src/six_point_two_eight.cppを修正しました。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/make_target_models.h"  // 追加。
#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/point_cloud_2_throttle.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeTargetModels, nodelet::Nodelet)  // 追加。
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)

six_point_two_eight.xml

続けて、プログラムを起動できるようにsix_point_two_eight.xmlを修正しました。

<library path="lib/libsix_point_two_eight">
  <class
    name="six_point_two_eight/make_target_models"
    type="six_point_two_eight::MakeTargetModels"
    base_class_type="nodelet::Nodelet"/>  <!-- 追加 -->
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>
</library>

launch/make_target_models.launch

起動を簡単にするために、launchファイルも作成しました。

<launch>
  <arg name="odom" default="odom"/>
  <arg name="velocity" default="mobile_base/commands/velocity"/>
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_target_models" args="load six_point_two_eight/make_target_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="odom" to="$(arg odom)"/>
    <remap from="velocity" to="$(arg velocity)"/>
  </node>
</launch>

launch/make_target_models_in_gazebo.launch

シミュレーター用のlaunchファイルも。

<launch>
  <include file="$(find six_point_two_eight)/launch/make_target_models.launch"/>
</launch>

actionlib

先ほどのコールバック地獄まっしぐらなプログラムを、ここで何とかしましょう。サービスを使えば解決できそうな気がしますけど、サービスは柔軟性が低いのでできれば使いたくない。幸いなことに、ROSにはactionlibという、中断することが可能な、長い時間をかけて実行するサーバー・プログラムを作成するためのパッケージがあって、このactionlibを使えばコールバック地獄をキレイに解決できるんです。

地図作成と自律移動のパッケージを動かして、ROSの可能性を感じてみる

申し訳ないのですけれどactionlibはとりあえず脇に置いて、一時的に本稿から離れてBuild a map with SLAMとAutonomously navigate in a known mapを開いて、指示に従ってみてください（もっと細かい情報が欲しいという場合は、Learn TurtleBot and ROSも参照してください）。

これらのページの指示に従ってコマンドを入力していくと、TurtleBotに地図を作成させ、作成した地図を活用して自律走行させることができます。しかも、必要な作業はパッケージの起動だけ。プログラミングは無しです。

本稿はここまでROSを通信ライブラリとして扱ってきましたけど、実は、ROSの凄さは2,000を超えるROS対応パッケージ群にあります。しかも、それらのパッケージはメッセージとトピックという抽象的なレベルでつながるのですから、様々なロボットで使用可能です。先ほどのページでも、最終的には汎用パッケージを使用しています。ページ中の呼び出しに使用しているturtlebot_navigationパッケージは、汎用のgmappingパッケージ¹⁰とnavigationパッケージを呼び出すためのlaunchファイル¹¹が詰まったパッケージなんです。

時間があるときに、ROS.orgのBrowse Softwareを見てみてください。私のお気に入りはhector_slamとnavigation。これらのパッケージの紹介ビデオを見れば、自律移動するロボットを簡単に作れることが分かって、ROSがいかに便利なものなのかを感じ取れるはずです。あと、MoveIt!もスゴイです。ロボット・アームがグリグリして、なんでもできちゃいそう。

• hector_slam

• navigation

actionlib::SimpleActionClient

先ほどのTurtleBotの自律移動のチュートリアルでは移動先をRVizのGUIで指定していましたけれど、一味違う我々は、プログラムから指定してみましょう。turtlebot_navigationパッケージが使用しているnavigationパッケージのドキュメントのSending Goals to the Navigation Stackに、移動先をプログラムから指定する方法が書いてありました。そして、このドキュメントの途中には、actionlibを使うと書いてあります。つまり、navigationに指示を出すプログラムを組めば、actionlibを試せるというわけ。

とってもお得な話ですから、さっそく試してみましょう。

package.xmlとCMakeLists.txt

「Sending Goals to the Navigation Stack」に必要だと書いてあるmove_base_msgsパッケージとactionlibパッケージを、依存するパッケージに追加してください。

launch/make_target_models.launch

使用するトピックが変更になりますので、launchファイルを変更します。

<launch>
  <arg name="move_base" default="move_base"/>  <!-- 追加。他のargは削除しました。 -->
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_target_models" args="load six_point_two_eight/make_target_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="move_base" to="$(arg move_base)"/>  <!-- 追加。他のremapは削除しました。 -->
  </node>
</launch>

include/six_point_two_eight/utilities.h

actionlibは、通常のROSのメッセージ通信を活用して、長時間処理を実現しています。だからプログラムから使用するのはメッセージです。どんなメッセージがあるのかを確認するために、ターミナルでrosmsg show move_base_msgs/MoveBaseまで入力して[Tab]キーを押してみてください。MoveBaseを実現するのに必要なメッセージが一覧表示されます。

$ rosmsg show move_base_msgs/MoveBase
move_base_msgs/MoveBaseAction          move_base_msgs/MoveBaseActionResult    move_base_msgs/MoveBaseResult
move_base_msgs/MoveBaseActionFeedback  move_base_msgs/MoveBaseFeedback
move_base_msgs/MoveBaseActionGoal      move_base_msgs/MoveBaseGoal

この中で我々が中身を知らなければならないのは、move_base_msgs/MoveBaseGoalメッセージとmove_base_msgs/MoveBaseResultメッセージとmove_base_msgs/MovebaseFeedbackメッセージ。ゴール（何をするかの指示）と結果と途中経過のフィードバックですね。他のメッセージは動作を制御するためのもので、ライブラリがうまいこと隠蔽してくれます。

utilities.hでは、とりあえず移動先を指定するためのMoveBaseGoalメッセージを作成する関数を追加しました。MoveBaseGoalメッセージを構成するgeometry_msgs/PoseStampedとgeometry_msgs/Pose、geometry_msgs/Quaternionを生成する関数も、合わせて作成しておきます。

#pragma once

#include <cmath>
#include <tf/transform_datatypes.h>  // 追加。

#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseGoal.h>  // 追加。<geometry_msgs/Pose.h>は削除。

namespace six_point_two_eight {
  // 略。

  // move_base_msgs/MoveBaseGoalに必要なgeometry_msgs/PoseStampedに必要なgeometry_msgs/Poseに必要なgeometry_msgs/Quaternionを作成します。
  inline auto createQuaternionMsg(double yaw) {
    return tf::createQuaternionMsgFromYaw(yaw);
  }
  
  // move_base_msgs/MoveBaseGoalに必要なgeometry_msgs/PoseStampedに必要なgeometry_msgs/Poseを作成します。
  inline auto createPoseMsg(double x, double y, double yaw) {
    geometry_msgs::Pose msg;
    msg.position = createPointMsg(x, y);
    msg.orientation = createQuaternionMsg(yaw);

    return msg;
  }
  
  // move_base_msgs/MoveBaseGoalに必要なgeometry_msgs/PoseStampedを作成します。
  inline auto createPoseStampedMsg(
    const std::string& frame_id, const ros::Time& stamp, double x, double y, double yaw) 
  {
    geometry_msgs::PoseStamped msg;
    msg.header.frame_id = frame_id;
    msg.header.stamp = stamp;
    msg.pose = createPoseMsg(x, y, yaw);

    return msg;
  }

  // move_base_msgs/MoveBaseGoalを作成します。
  inline auto createMoveBaseGoalMsg(
    const std::string& frame_id, const ros::Time& stamp, double x, double y, double yaw) 
  {
    move_base_msgs::MoveBaseGoal msg;
    msg.target_pose = createPoseStampedMsg(frame_id, stamp, x, y, yaw);

    return msg;
  }

  // 略。
}

include/six_point_two_eight/make_target_models.h

MoveBaseActionを呼び出す形で、six_point_two_eight::MakeTargetModelsクラスを全面的に書きなおしました。actionlibなし版と比較できるようにお題は同じえ、9時の方向に0.5m進みます。

#pragma once

#include <actionlib/client/simple_action_client.h>  // 追加。
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>  // 追加。

#include <move_base_msgs/MoveBaseAction.h>  // 追加。

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    std::thread working_thread_;

    auto moveTo(const move_base_msgs::MoveBaseGoal& goal) {
      // actionlibのクライアントを作成します。
      actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> action_client("move_base", true);

      // actionlibのサーバーの起動を待ちます。
      if (!action_client.waitForServer(ros::Duration(60.0))) {
        ROS_ERROR_STREAM("Timeout occured when waiting action server.");
        throw std::exception();
      }

      // actionlibのサーバーにゴールを送信します。
      action_client.sendGoal(goal);

      // actionlibのサーバーの処理が完了するのを待ちます。
      if (!action_client.waitForResult(ros::Duration(60.0))) {
        ROS_ERROR_STREAM("Timeout occured when waiting result.");
        throw std::exception();
      }

      // ステータスを調べます。
      if (action_client.getState() != actionlib::SimpleClientGoalState::StateEnum::SUCCEEDED) {
        ROS_ERROR_STREAM(action_client.getState().toString());
        throw std::exception();
      }

      // 結果を返します。
      return *action_client.getResult();
    }
    
  public:
    void onInit() {
      // onInitの中でループするとNodeletManagerの処理が止まってしまうので、別スレッドで実行します。
      working_thread_ = std::thread(
        [&]() {
          // base_linkの現在の座標系で、X=0、Y=0.5、向きはそのままになるように移動させる（9時の方向に0.5mで方向は変えない）。
          moveTo(createMoveBaseGoalMsg("base_link", ros::Time::now(), 0.0, 0.5, 0.0));

          // 移動が終わったら実行する処理を、ここに書くことができます！
          ROS_INFO_STREAM("Finished!");
        });
    }
  };
}

onInit()に注目してください。移動が完了した後の処理（今回はROS_INFO_STREAM()）が、moveTo()メンバー関数の呼び出しの後に書かれています。コールバック地獄が発生しない、実に見通しがよいコードです。actionlib便利すぎですね。

あと、移動先をどのフレームの座標系で示すかを指示できるので、三角関数が不要になりました。やっぱり便利すぎ。

actionlib::SimpleActionClient呼び出しを、ライブラリ化する

でも、先ほどのコードのactionlibのサーバーを呼び出す部分、書くのが面倒くさい……。ライブラリ化してしまいましょう。

include/six_point_two_eight/utilities.h

本稿の要件では、actionlibのフィードバックは不要です。同期処理にできるのですから、ファンクターで実装できるでしょう。早速やってみます。

#pragma once

#include <actionlib/client/simple_action_client.h>  // 追加。
#include <cmath>
#include <tf/transform_datatypes.h>

#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>  // 追加。<move_base_msgs/MoveBaseGoal.h>の#includeは削除。

namespace six_point_two_eight {
  // 本稿用の例外。
  class SixPointTwoEightException : public std::exception {
  private:
    std::string what_;

  public:
    SixPointTwoEightException(const std::string& what)
      : what_(what)
    {
      ;
    }

    const char* what() const noexcept {
      return what_.c_str();
    }
  };
  
  // 略。

  // actionlib呼び出し用ファンクター。
  template <typename Action>
  struct CallAction {
    // Goalの型。
    using Goal = typename Action::_action_goal_type::_goal_type;
    
    // Resultの型。
    using Result = typename Action::_action_result_type::_result_type;
    
    // Nodeletの初期化はonInit()なので、メンバー変数の初期化をやりづらい。なので、トピックはoperator()の引数にしました。
    // ただし、毎回引数にトピックを書くと保守性が低くなってしまうので、onInit()でトピックをstd::bindできるようにします。以下は、バインド後の変数のための型です。
    using TopicBinded = std::function<Result(const Goal&)>;

    // 何度もstd::bindを書くのは面倒なのでヘルパー用関数を用意します。この関数をonInit()の中で呼んでください。
    static auto bindTopic(const std::string& topic) {
      return std::bind(CallAction<Action>(), topic, std::placeholders::_1);
    }

    // 例外メッセージを生成します。
    auto exceptionWhat(const std::string& message, const std::string& topic) const {
      std::stringstream what;
      what << message << " topic = " << topic << ".";
      
      return what.str();
    }

    // actionlibを呼び出します。
    auto operator()(const std::string& topic, const Goal& goal) {
      actionlib::SimpleActionClient<Action> action_client(topic, true);
      if (!action_client.waitForServer(ros::Duration(60.0))) {
        throw SixPointTwoEightException(exceptionWhat("Timeout occured when waiting action server.", topic));
      }
      
      action_client.sendGoal(goal);
      if (!action_client.waitForResult(ros::Duration(60.0))) {
        throw SixPointTwoEightException(exceptionWhat("Timeout occured when waiting result.", topic));
      }
      
      if (action_client.getState() != actionlib::SimpleClientGoalState::StateEnum::SUCCEEDED) {
        throw SixPointTwoEightException(exceptionWhat(action_client.getState().toString(), topic));
      }
      
      return *action_client.getResult();
    }
  };

  // メンバー変数宣言とbindTopic()で2回クラス名を書くのは大変なので、usingしておきます。
  using CallMoveBaseAction = CallAction<move_base_msgs::MoveBaseAction>;
}

TopicBindedとbindTopic()のあたりが複雑になっているのは、Nodeletの初期化はonInit()で実施されるためです。メンバー変数の初期化をコンストラクターの初期化リストに書けない（初期化リストに書くと、初期化のコードが分散するので保守性が落ちてしまう）ので、トピックはoperator()の引数になります。でも呼び出しのたびにトピックを引数に書くのは面倒だし保守性が下がってしまうので、onInit()の中でトピックをstd::bindしておきたい。このstd::bindした結果を格納するメンバー変数の型がTopicBinded、std::bind呼び出しを少しだけ楽にするのがbindTopic()メンバー関数です。具体的な使い方は、次のmake_target_models.hでやります。

include/six_point_two_eight/make_target_models.h

作成したCallActionを使用する形に、MakeTargetModelsを書き直しましょう。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>

// <actionlibclient/simple_action-client.h>と<move_base_msgs/MoveBaseGoal.h>の#includeは削除。

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    // actionlibは、CallActionを経由して呼び出します。
    CallMoveBaseAction::TopicBinded move_base_;
    
    std::thread working_thread_;
    
  public:
    void onInit() {
      // トピックをバインドします。
      move_base_ = CallMoveBaseAction::bindTopic("move_base");
      
      working_thread_ = std::thread(
        [&]() {
          // actionlibを呼び出します。
          move_base_(createMoveBaseGoalMsg("base_link", ros::Time::now(), 0.0, 0.5, 0.0));
          
          ROS_INFO_STREAM("Finished!");
        });
    }
  };
}

とても簡単。actionlibは本当に便利ですな。

actionlib::SimpleActionServer

でも、対象物の周囲360°からの点群作成のための道具としては、navigationパッケージはちょっとオーバースペックかも。acitonlibの勉強がてら、同じ処理（ただし、地図を参照して最短経路を探したり障害物を避けたりしない。指定された座標にただ進むだけの低機能版）をするNodeletを作ってみましょう。

include/six_point_two_eight/move_base_server.h

actionlibのサーバーは、actionlib::SimpleActionServerクラスを使用して作成します。実際の処理をする関数を引数に渡して、処理が終了したらsetSucceeded()メンバー関数を呼ぶだけ。使い方そのものは簡単です。

ただし、このSimpleActionServerクラス、初期化はコンストラクタでやることになっていて、しかもoperator=が無効になっているんですよ。このプログラミング・スタイルそのものは、良いプログラミング・スタイルだと考えます。不完全なインスタンスは危険ですから、初期化をコンストラクタで完全にやっておくのは良いことでしょう。そのクラスをメンバー変数に持ちたい時は、コンストラクタの初期化リストで初期化すればよい。初期化リストでメンバー変数を初期化するのは、良いスタイルだとされていますしね。変数への再代入を禁止しておけば状態遷移がなくなって、関数型言語のコードみたいに単純になるでしょう。と、感じに良いこと尽くめなプログラミング・スタイルなのですけれど、残念なことにNodeletと相性が悪いんです……。

Nodeletの場合、SimpleActionServerをコンストラクタの初期化リストで初期化するのは駄目です。NodeletのonInit()がまだ呼ばれていませんからね。operator=が無効になっているので、onInit()の中での初期化も不可能です。

class MoveBaseServer : public nodelet::Nodelet {
private:
  actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> move_base_action_server_;
  
public:
  MoveBaseServer()
    : move_base_action_server_(getNodeHandle(), "move_base", false)  // NG。Nodeletの初期化がなされていないのに、getNodeHandle()を呼んでいる。
  {
    ;
  }
  
  void onInit() {
    velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);
    
    // 以下はコンパイル・エラーになります。
    // move_base_action_server_ = actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction>(getNodeHandle(), "move_base", false);
    
    ...
  }
};

困った時のGoogle先生ということで、インターネットを検索したら別クラス作る方式のコードがありました。以下のような感じ。

class MoveBaseServerImpl {
private:
  actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> move_base_action_server_;

public:
  MoveBaseServerImpl(ros::NodeHandle* node_handle)
    : move_base_action_server_(*node_handle, "move_base", false)
  {
    // move_base_action_server_を使う処理。
  }
};

class MoveBaseServer {
private:
  boost::optional<MoveBaseServerImpl> impl_;
  
public:
  void onInit() {
    velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

    impl_ = boost::in_place(&getNodeHandle());
  }
};

ただ、このコードだと、ちょっと不格好ですね……。処理が分散していて、保守性が低そうです。

でも、このコードはよいヒントになります。このコードで使われているboostのIn-Place Factoryなら、初期化を遅らせることが可能なわけ（このコードを見るまで、私はIn-Place Factoryを知りませんでした）。だったら、間にクラスを挟まなくてもよいはず。というわけで、以下のように、In-Place Factoryを使ってコードを書きました。

移動処理のほとんどは、以前作成したactionlibを使わないバージョンからのコピー＆ペーストです。回転する処理と、/odom以外のフレームが指定された時に備えるためにTFで座標変換する処理を付け加えた程度です。

#pragma once

#include <actionlib/server/simple_action_server.h>
#include <boost/utility/in_place_factory.hpp>
#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <move_base_msgs/MoveBaseAction.h>

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MoveBaseServer : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber odom_subscriber_;

    // MoveBaseActionのサーバーです。boostのIn-Place Factoryを使用して、初期化を遅らせます。
    boost::optional<actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction>> move_base_action_server_;
    
    auto tunedLinearX(double distance, double angle) const {  // 前に作成したものと同じ。
      if (distance < 0.05) {
        return 0.0;
      }
      if (std::fabs(angle) > M_PI / 180 * 45) {
        return 0.0;
      }
      
      return std::min(std::fabs(distance) * 0.5, 0.1);
    }
    
    auto tunedAngularZ(double angle) const {  // 以前作成したものと同じ。
      if (std::fabs(angle) < M_PI / 180 * 1) {
        return 0.0;
      }
      
      return std::min(std::fabs(angle) * 0.5, M_PI / 180 * 10) * (angle / std::fabs(angle));
    }

    // ゴールの向きと同じなるように回転します（作業用）。
    auto turnTo(const geometry_msgs::Pose& goal_pose, nav_msgs::OdometryConstPtr odometry) {
      // ゴールの向きと現在の向きの差を計算します。
      auto angular_z = this->tunedAngularZ(
        normalizeAngle(tf::getYaw(goal_pose.orientation) - tf::getYaw(odometry->pose.pose.orientation)));

      // ロボットを回転させます。
      velocity_publisher_.publish(createTwistMsg(0, angular_z));

      // 回転したかどうかを返しhます。
      return angular_z != 0.0;
    }

    // ゴールの向きと同じなるように回転します。
    auto turnTo(const geometry_msgs::Pose& goal_pose) {
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&, goal_pose](const auto& message) {
          if (this->turnTo(goal_pose, message)) {  // 回転した場合は、それで今回分の処理は終了なので、
            return;                                // そのままリターンします。
          }

          move_base_action_server_->setSucceeded(move_base_msgs::MoveBaseResult());  // 回転しない＝終了なので、MoveBaseActionを終了させます。
          odom_subscriber_.shutdown();  // サブスクライバーをシャットダウンします。
        });
    }
    
    auto moveTo(const geometry_msgs::Pose& goal_pose, nav_msgs::OdometryConstPtr odometry) {  // 以前作成したものから、1つ目の引数を変えただけ。
      auto difference = createPointMsg(
        goal_pose.position.x - odometry->pose.pose.position.x, goal_pose.position.y - odometry->pose.pose.position.y);
      
      auto distance = std::sqrt(std::pow(difference.x, 2) + std::pow(difference.y, 2));
      auto angle =
        normalizeAngle(std::atan2(difference.y, difference.x) - tf::getYaw(odometry->pose.pose.orientation));
      
      auto linear_x = this->tunedLinearX(distance, angle);
      auto angular_z = this->tunedAngularZ(angle);
      
      velocity_publisher_.publish(createTwistMsg(linear_x, angular_z));
      
      return linear_x != 0.0 || angular_z != 0.0;
    }

    auto moveTo(const geometry_msgs::Pose& goal_pose) {
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&, goal_pose](const auto& message) {
          if (this->moveTo(goal_pose, message)) {
            return;
          }

          this->turnTo(goal_pose);  // 撮影ポイントまで移動した後は、対象物が正面に来るように回転します。
        });
    }

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      // boostのIn-Place Factoryを使用して、サーバーを初期化します。
      move_base_action_server_ = boost::in_place(getNodeHandle(), "move_base", false);
      
      // Goalが送られてきた場合の処理を登録します。
      move_base_action_server_->registerGoalCallback(
        [&]() {
          // acceptNewGoal()でゴールを取得します。
          auto goal = move_base_action_server_->acceptNewGoal();

          // TFで座標変換します。
          geometry_msgs::PoseStamped goal_pose_stamped;
          transform_listener_.transformPose("odom", goal->target_pose, goal_pose_stamped);

          // 実際の処理を担当するのメンバー関数を呼び出します。
          moveTo(goal_pose_stamped.pose);
        });

      // サーバーを開始します。
      move_base_action_server_->start();
    }
  };
}

src/six_point_two_eight.cpp

move_base_server.hがコンパイルされるように、*.cppを修正します。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/make_target_models.h"
#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/move_base_server.h"  // 追加。
#include "six_point_two_eight/point_cloud_2_throttle.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeTargetModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MoveBaseServer, nodelet::Nodelet)  // 追加。
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)

six_point_two_eight.xml

作成したsix_point_two_eight::MoveBaseServerをROSから扱えるように、名前を付けます。

<library path="lib/libsix_point_two_eight">
  <class name="six_point_two_eight/make_target_models" type="six_point_two_eight::MakeTargetModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <!-- ここから追加。 -->
  <class
    name="six_point_two_eight/move_base_server"
    type="six_point_two_eight::MoveBaseServer"
    base_class_type="nodelet::Nodelet"/>
  <!-- ここまで。 -->
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>
</library>

include/six_point_two_eight/make_target_models.h

ついでですから、対象物の周囲360°を回るように、make_target_models.hを修正してしまいましょう。対象物は正面にあるものとし、その対象物の周囲の撮影ポイントを回ります。/odomを使用して、絶対位置で計算するようにしました。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <nav_msgs/Odometry.h>  // 追加。<thread>の#includeは削除。

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    static constexpr double radius = 1.25;  // 対象物の周囲を回る円の半径。
    static const int step_size = 16;        // 1周回る間に撮影する回数。

    ros::Subscriber odom_subscriber_;
    CallMoveBaseAction::TopicBinded move_base_;
    
  public:
    void onInit() {
      move_base_ = CallMoveBaseAction::bindTopic("move_base");

      // 相対座標だとプログラムが複雑になるので、/odomを使用して絶対座標で位置決めをします。
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          // 自機の絶対位置。
          auto position = message->pose.pose.position;
          auto yaw = tf::getYaw(message->pose.pose.orientation);

          // ターゲットの絶対位置を計算します。
          auto target_position = createPointMsg(position.x + std::cos(yaw) * radius, position.y + std::sin(yaw) * radius);
          auto target_yaw = normalizeAngle(yaw + M_PI);  // ターゲットから見た、自機の方向。

          // ターゲットの周囲を回ります。
          for (auto i = 0; i < step_size; ++i) {
            ROS_INFO_STREAM("Step " << (i + 1) << "/" << step_size);

            // 次の撮影位置に移動します。
            target_yaw = normalizeAngle(target_yaw + M_PI * 2 / step_size);
            move_base_(
              createMoveBaseGoalMsg(
                "odom",
                ros::Time::now(),
                target_position.x + std::cos(target_yaw) * radius,
                target_position.y + std::sin(target_yaw) * radius,
                normalizeAngle(target_yaw + M_PI)));
          }

          ROS_INFO_STREAM("Finished!");
          odom_subscriber_.shutdown();
        });
    }
  };
}

actionlibのおかげで、移動→回転→移動……をループで表現出来ました。やっぱりactionlibは便利ですね。

launch/make_target_models.launch

最後。動作確認をするために、起動用のlaunchファイルを修正しましょう。、

<launch>
  <arg name="move_base" default="move_base"/>
  <arg name="odom" default="odom"/>  <!-- 追加 -->
  <arg name="velocity" default="mobile_base/commands/velocity"/>  <!-- 追加 -->
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_target_models" args="load six_point_two_eight/make_target_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="move_base" to="$(arg move_base)"/>
  </node>
  <!-- ここから追加。 -->
  <node
    pkg="nodelet"
    type="nodelet"
    name="move_base_server"
    args="load six_point_two_eight/move_base_server six_point_two_eight_nodelet_manager"
    output="screen">
    <remap from="odom" to="$(arg odom)"/>
    <remap from="velocity" to="$(arg velocity)"/>
  </node>
  <!-- ここまで。 -->
</launch>

で、実際に動かしてみると、移動が遅くて完了までかなり長い時間がかかります……。これは、オドメトリーの誤差をできるだけ減らすためです。navigationパッケージで速く動けているのは、あれは深度センサーの情報を元に現在位置を補正し続けているからなんです。今回はそんな高度な処理は作成していませんから、申し訳ないですけど、遅いのは我慢してください。

*.actionの定義

点群を取得する処理も、actionlib化してしまいましょう。ターミナルでrosmsg list | grep ActionGoalしてみても使えそうな既存のアクションはなさそうなので、新しいアクションの定義方法を学ぶこともできますしね。

action/GetPointCloud2.action

アクションは、actionディレクトリの下に、*.actionとして定義してください。ファイル名がそのままアクション名になります。*.actionの中は、---で3つに区切られます。区切り方は、以下のような感じ。

Goalのメッセージ定義
---
Resultのメッセージ定義
---
Feedbackのメッセージ定義

で、メッセージ定義は、型 変数名の0行以上の繰り返し。何もなくてよい（move_base_msgs/MoveBaseResultのような）場合は、0行になります。今回は、どのトピックからデータを取得するのかを渡して、PointCloud2を返す感じ。途中経過がないので、フィードバックは無しで。なので、GetPointCloud2.actionの中身は以下になります。

string target_topic
---
sensor_msgs/PointCloud2 points
---

package.xmlとCMakeLists.xml

メッセージ定義の場合は、package.xmlの<build_depend>にmessage_generation、<run_depend>にmessage_runtimeを指定するという面倒なルールがあるのですけれど、アクション定義の場合はどちらもactionlib_msgsパッケージで大丈夫です。actionlib_msgsをpackage.xmlの<build_depends>と<run_depends>に追加してください。

CMakeLists.xmlはもう少し複雑です。というのも、*.actionをコンパイル（といってもC++やPython、Common Lispのコード生成）するように指示しなければならないんです。あと、*.ationのコンパイルが終了してC++のコードが生成されるまで、*.cppのビルドを待つようにも指示します。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  actionlib
  actionlib_msgs
  geometry_msgs
  move_base_msgs
  nav_msgs
  nodelet
  nodelet_topic_tools
  pcl_ros
  roscpp
  sensor_msgs
  tf
  tf_conversions
)

# find_package(Boost REQUIRED COMPONENTS system)

# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

# *.actionを登録します。
add_action_files(FILES
  GetPointCloud2.action
)

# メッセージを生成するように指示します。add_xxx_filesに加えて、generate_massagesも必要です。
generate_messages(DEPENDENCIES  # 追加。
  actionlib_msgs                # 追加。\*.actionが参照するメッセージを含むパッケージを書きます。
  sensor_msgs                   # 追加。\*.actionが参照するメッセージを含むパッケージを書きます。
)                               # 追加。

catkin_package(CATKIN_DEPENDS
  actionlib_msgs  # 追加。ここには、catkinの実行に必要なパッケージを設定します。
)

add_compile_options(
  "-std=c++14"
)

set_source_files_properties(src/point_cloud_utilities.cpp
  PROPERTIES COMPILE_FLAGS "-std=c++03 -Wno-deprecated"
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_library(six_point_two_eight
  src/six_point_two_eight.cpp
  src/point_cloud_utilities.cpp
  src/utilities.cpp
)

# メッセージの生成が終わるまで（*.hが生成されるまで）、*.cppのビルドを待つように指示します。
add_dependencies(six_point_two_eight ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(six_point_two_eight
  ${catkin_LIBRARIES}
)

ここまで書いたらターミナルでcatkin_makeしてみてください。<catkin_ws>/devel/include/six_point_two_eightディレクトリに以下のファイルができていればOKです。

$ ls devel/include/six_point_two_eight
GetPointCloud2ActionFeedback.h  GetPointCloud2Action.h        GetPointCloud2Feedback.h   GetPointCloud2Result.h
GetPointCloud2ActionGoal.h      GetPointCloud2ActionResult.h  GetPointCloud2Goal.h

include/six_point_two_eight/utilities.h

いつものutilities.hに、メッセージ生成関数とCallActionの別名定義を追加しておきます。

six_point_two_eightパッケージの中で定義したメッセージのの名前空間は、six_point_two_eightになります。あと、includeファイルはsix_point_two_eight/メッセージ名.h。一般には、我々がmove_base_msgs/MoveBaseActionを再利用したようにメッセージだけが再利用される可能性もありますから、*_msgsという名前のパッケージを作ってそこにメッセージを定義すべきです。でもまぁ、今回のメッセージはsix_point_two_eightパッケージ内でしか利用しないでしょうから、ごめんなさい、パッケージを分けませんでした。

#pragma once

#include <actionlib/client/simple_action_client.h>
#include <cmath>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <six_point_two_eight/GetPointCloud2Action.h>  // 追加。

namespace six_point_two_eight {
  // 略。
  
  // GetPointCloud2Goalメッセージを生成します。
  inline auto createGetPointCloud2GoalMsg(const std::string& target_topic) {
    six_point_two_eight::GetPointCloud2Goal msg;
    msg.target_topic = target_topic;

    return msg;
  }

  // GetPointCloud2Resultメッセージを生成します。
  inline auto createGetPointCloud2ResultMsg(sensor_msgs::PointCloud2ConstPtr points) {
    six_point_two_eight::GetPointCloud2Result msg;
    msg.points = *points;

    return msg;
  }

  // 利便性のために、別名を定義しておきます。
  using CallGetPointCloud2Action = CallAction<six_point_two_eight::GetPointCloud2Action>;

  // 略。
}

include/six_point_two_eight/get_point_cloud_server.h

GetPointCloud2アクションを担当するNodeletを作ります。これまでに学習した知識（と作成したコードのコピー＆ペーストと少しの修正）で、サクッと作りました。

#pragma once

#include <actionlib/server/simple_action_server.h>
#include <boost/utility/in_place_factory.hpp>
#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <six_point_two_eight/GetPointCloud2Action.h>

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class GetPointCloud2Server : public nodelet::Nodelet {
  private:
    ros::Subscriber points_subscriber_;
    boost::optional<actionlib::SimpleActionServer<six_point_two_eight::GetPointCloud2Action>> get_point_cloud_2_action_server_;
    
    auto getPointCloud2(const std::string& target_topic) {
      auto points_subscribing_time = ros::Time::now();
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        target_topic, 1,
        [&, points_subscribing_time](const auto& message) {
          // 起動直後は動作が安定しないので、3秒間は何もしません。
          if (message->header.stamp < points_subscribing_time + ros::Duration(3.0)) {
            return;
          }
          
          get_point_cloud_2_action_server_->setSucceeded(createGetPointCloud2ResultMsg(message));
          points_subscriber_.shutdown();
        });
    }

  public:
    void onInit() {
      get_point_cloud_2_action_server_ = boost::in_place(getNodeHandle(), "get_point_cloud_2", false);
      get_point_cloud_2_action_server_->registerGoalCallback(
        [&]() {
          getPointCloud2(get_point_cloud_2_action_server_->acceptNewGoal()->target_topic);
        });
      get_point_cloud_2_action_server_->start();
    }
  };
}

src/six_point_two_eight.cpp

move_base_server.hの時と同じ、単純作業です。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/get_point_cloud_2_server.h"  // 追加。
#include "six_point_two_eight/make_target_models.h"
#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/move_base_server.h"
#include "six_point_two_eight/point_cloud_2_throttle.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::GetPointCloud2Server, nodelet::Nodelet)  // 追加。
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeTargetModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MoveBaseServer, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)

six_point_two_eight.xml

単純作業が続いて申し訳ありませんが、Nodeletに名前を付けてあげてください。

<library path="lib/libsix_point_two_eight">
  <!-- ここから追加。 -->
  <class
    name="six_point_two_eight/get_point_cloud_2_server"
    type="six_point_two_eight::GetPointCloud2Server"
    base_class_type="nodelet::Nodelet"/>
  <!-- ここまで。 -->
  <class name="six_point_two_eight/make_target_models" type="six_point_two_eight::MakeTargetModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/move_base_server" type="six_point_two_eight::MoveBaseServer" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>
</library>

include/six_point_two_eight/make_target_models.h

作成したactionlibを呼び出すmake_world_models.hも、修正しましょう。

point_cloud_utilities.hの関数はsensor_msgs::PointCloud2ConstPtrを引数にとる形で定義していて、six_point_two_eight::GetPointCloud2Resultから受け取れるデータはsensor_msgs::PointCloud2（ConstPtrがつかない）なので、型変換しています。あと、point_cloud_utilities.hの関数を呼び出した時に例外が発生してもデータを取り直せるように、whileループで囲んでいます。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>  // 追加。

#include "six_point_two_eight/point_cloud_utilities.h"  // 追加。
#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    static constexpr double radius = 1.25;
    static const int step_size = 16;

    ros::Subscriber odom_subscriber_;
    CallGetPointCloud2Action::TopicBinded get_point_cloud_2_;  // 追加。
    CallMoveBaseAction::TopicBinded move_base_;

  public:
    void onInit() {
      get_point_cloud_2_ = CallGetPointCloud2Action::bindTopic("get_point_cloud_2");  // 追加。
      move_base_ = CallMoveBaseAction::bindTopic("move_base");

      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          auto position = message->pose.pose.position;
          auto yaw = tf::getYaw(message->pose.pose.orientation);

          auto target_position = createPointMsg(position.x + std::cos(yaw) * radius, position.y + std::sin(yaw) * radius);
          auto target_yaw = normalizeAngle(yaw + M_PI);

          // ターゲットの周囲を、深度センサーからデータを取得しながら回ります。
          for (auto i = 0; i < step_size; ++i) {
            ROS_INFO_STREAM("Step " << (i + 1) << "/" << step_size);

            // ここから追加。
            // 深度センサーからデータを取得します。
            while (ros::ok()) {
              try {
                savePointCloud2File(
                  getSpherePointCloud2(
                    transformPointCloud2(
                      sensor_msgs::PointCloud2ConstPtr(  // PointCloud2をPointCloud2ConstPtrに変換します。
                        new sensor_msgs::PointCloud2(
                          get_point_cloud_2_(createGetPointCloud2GoalMsg("points")).points)),  // アクションから点群を取得。
                      "odom"),
                    target_position,
                    radius * 0.5));
                break;
              
              } catch (const std::exception& ex) {
                ROS_WARN_STREAM(ex.what());
              }
            }
            // ここまで。
            
            target_yaw = normalizeAngle(target_yaw + M_PI * 2 / step_size);
            
            move_base_(
              createMoveBaseGoalMsg(
                "odom",
                ros::Time::now(),
                target_position.x + std::cos(target_yaw) * radius,
                target_position.y + std::sin(target_yaw) * radius,
                normalizeAngle(target_yaw + M_PI)));
          }

          ROS_INFO_STREAM("Finished!");
          odom_subscriber_.shutdown();
        });
    }
  };
}

launch/make_target_models.launch

launchファイルも修正します。せっかくNodeletを作っても、起動させなければ無意味ですもんね。

<launch>
  <arg name="get_point_cloud_2" default="get_point_cloud_2"/>
  <arg name="move_base" default="move_base"/>
  <arg name="odom" default="odom"/>
  <arg name="points" default="camera/depth_registered/points"/>  <!-- 追加 -->
  <arg name="velocity" default="mobile_base/commands/velocity"/>
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_target_models" args="load six_point_two_eight/make_target_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="get_point_cloud_2" to="$(arg get_point_cloud_2)"/>
    <remap from="move_base" to="$(arg move_base)"/>
  </node>
  <!-- ここから追加。 -->
  <node
    pkg="nodelet"
    type="nodelet"
    name="get_point_cloud_2_server"
    args="load six_point_two_eight/get_point_cloud_2_server six_point_two_eight_nodelet_manager"
    output="screen">
    <remap from="points" to="$(arg points)"/>
  </node>
  <!-- ここまで。 -->
  <node pkg="nodelet" type="nodelet" name="move_base_server" args="load six_point_two_eight/move_base_server six_point_two_eight_nodelet_manager" output="screen">
    <remap from="odom" to="$(arg odom)"/>
    <remap from="velocity" to="$(arg velocity)"/>
  </node>
</launch>

launch/make_target_models_in_gazebo.launch

今回の処理は点群を扱いますから、シミュレーター用のlaunchファイルも修正しておきます。

<launch>
  <include file="$(find six_point_two_eight)/launch/make_target_models.launch">
    <arg name="points" value="camera/depth/points"/>
  </include>
</launch>

PCLを使用して、床を除去する

知識と作成済みコードが増えた結果、なんだか大抵の作業が機械的にできちゃってつまんない。少し新しいことやりたいな……。というわけで、点群の処理をやってみましょう。床の除去です。

床を除去？

今回のプログラムでは床を除去する意味はあまりないのですけれど、ロボット・アームを使う場合等では、床の除去はとても重要です。ロボット・アームでテーブルの上にあるモノを掴むとき、テーブル板を除去できれば、どこにどんなモノがあるのかを簡単に判定できるようになりますから。

PCLは当然この機能を提供していて、チュートリアルのPlane model segmentationで、平面を検出する方法を紹介しています。点群を分割する、セグメンテーションの機能で平面を検出しています。このセグメンテーション機能を使って、やってみましょう¹²。

セグメンテーション

ただね、PCLが提供する機能は、点群の中で一番大きな平面の検出だけなんですよ。たとえば、平らな壁も、平面として識別されてしまいます。対象物に平面がある場合も危険です。幸いなことに平面を識別する機能では見つかった平面の式の係数（平面の方程式ax + by + cz + d = 0のaとb、c、d）を返してくれます。aとbが小さくてcが大きいなら水平面だと考えられますから、床と壁を区別できそう。dが小さければ位置が低いので、対象物の平面ではなさそう。というわけで、ロジックが決まりました。

include/six_point_two_eight/point_cloud_utilities.h

作成する関数の名前は、removeFloorFromPointCloud2としました。

#ifndef SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H
#define SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H

#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size);
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius);
  sensor_msgs::PointCloud2Ptr removeFloorFromPointCloud2(sensor_msgs::PointCloud2ConstPtr points);  // 追加。
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

#endif

src/point_cloud_utilities.cpp

床を削除するコードは、こんな感じ。床ではない平面が検出された場合は次の平面を探さなければなりませんので、再帰呼出ししています。平面の数はそれほど多くないでしょうから、再帰呼出しでもスタックが不足することはないでしょう。また、点を削除するときにインデックスが変わらないように、setKeepOrganized()にtrueを設定できるようにもしてあります。

あと、ごめんなさい。以前のコードはグローバルな名前空間を汚していて嫌だっとので、今回、実装部分の関数をpoint_cloud_utilities名前空間で囲んでみました。

#include <boost/foreach.hpp>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>  // 追加。
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>  // 追加。
#include <pcl/sample_consensus/model_types.h>  // 追加。
#include <pcl/segmentation/sac_segmentation.h>  // 追加。
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

#include "six_point_two_eight/point_cloud_utilities.h"

namespace point_cloud_utilities {  // グローバルな名前空間を汚さないように、名前空間で囲みました。
  // 略。

  // 点群からインデックス群で示される点を除去します。
  PointCloud::Ptr removeIndicesFromPointCloud(PointCloud::ConstPtr point_cloud, pcl::PointIndices::ConstPtr point_indices, bool keep_organized = false) {
    // インデックス群抽出のpcl::ExtractIndicesを生成します。
    pcl::ExtractIndices<Point> extract_indices;
    extract_indices.setInputCloud(point_cloud);
    extract_indices.setIndices(point_indices);
    extract_indices.setKeepOrganized(keep_organized);  // 点を削除するか、NaNとして保持しておくかどうか。
    extract_indices.setNegative(true);  // trueにすると、インデックス群以外を残します。falseにするとインデックス群を残す（切り抜き）になります。デフォルトはfalse。

    // インデックス群を除去します。
    PointCloud::Ptr indices_removed_point_cloud(new PointCloud());
    extract_indices.filter(*indices_removed_point_cloud);

    return indices_removed_point_cloud;
  }

  // 点群の中の、床だと思われる点のインデックス群を取得します。
  pcl::PointIndices::Ptr getFloorPointIndices(PointCloud::ConstPtr point_cloud) {
    // 再帰呼出しの終了条件その1。点群に点がなくなったら再帰呼び出しを終了します。
    if (point_cloud->size() == 0) {
      return pcl::PointIndices::Ptr();
    }
    
    // 平面検出用のpcl::SACSegmentationを生成します。
    pcl::SACSegmentation<Point> segmentation;
    segmentation.setOptimizeCoefficients(true);
    segmentation.setModelType(pcl::SACMODEL_PLANE);
    segmentation.setMethodType(pcl::SAC_RANSAC);
    segmentation.setDistanceThreshold(0.025);
    segmentation.setInputCloud(point_cloud);

    // 平面を検出します。
    pcl::ModelCoefficients::Ptr floor_model_coefficients(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr floor_point_indices(new pcl::PointIndices());
    segmentation.segment(*floor_point_indices, *floor_model_coefficients);

    // 平面を検出できなかった場合の処理。
    if (floor_point_indices->indices.size() == 0) {
      ROS_WARN_STREAM("Can't find plane...");
      return pcl::PointIndices::Ptr();
    }

    // 検出した平面が床ではないと思われる場合の処理。
    if (std::abs(floor_model_coefficients->values[0]) > 0.2 ||
        std::abs(floor_model_coefficients->values[1]) > 0.2 ||
        floor_model_coefficients->values[2] < 0.9 ||
        std::abs(floor_model_coefficients->values[3] > 0.2))
    {
      ROS_WARN_STREAM(
        "Can't find floor... Coefficients are " <<
        floor_model_coefficients->values[0] <<
        ", " <<
        floor_model_coefficients->values[1] <<
        ", " <<
        floor_model_coefficients->values[2] <<
        " and " <<
        floor_model_coefficients->values[3]);

      // 床ではない平面を除去して、再帰呼出しします。
      return getFloorPointIndices(removeIndicesFromPointCloud(point_cloud, floor_point_indices, true));
    }

    // 床と思われる平面を返す。再帰呼出しは完了します。
    return floor_point_indices;
  }

  // 点群から床を除去します。
  PointCloud::Ptr removeFloorFromPointCloud(PointCloud::ConstPtr point_cloud) {
    // 床のインデックス群を取得します。
    pcl::PointIndices::Ptr floor_point_indices = getFloorPointIndices(point_cloud);
    if (!floor_point_indices) {
      ROS_WARN_STREAM("Can't find floor...");
      throw std::exception();
    }

    // 床のインデックスの点を除去します。
    return removeIndicesFromPointCloud(point_cloud, floor_point_indices);
  }

  // 略。
}

// 処理関数を名前空間で囲んだので、以下の関数群の実装に、point_cloud_utiliites::を付けました。

sensor_msgs::PointCloud2Ptr six_point_two_eight::downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::downsamplePointCloud(point_cloud_utilities::fromROSMsg(points), leaf_size));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::getSpherePointCloud(point_cloud_utilities::fromROSMsg(points), Eigen::Vector3f(center.x, center.y, center.z), radius));
}

// ここから追加。
sensor_msgs::PointCloud2Ptr six_point_two_eight::removeFloorFromPointCloud2(
  sensor_msgs::PointCloud2ConstPtr points) 
{
  return 
    point_cloud_utilities::toROSMsg(
      point_cloud_utilities::removeFloorFromPointCloud(
        point_cloud_utilities::fromROSMsg(
          points)));
}
// ここまで。

sensor_msgs::PointCloud2Ptr six_point_two_eight::savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::savePointCloudFile(point_cloud_utilities::fromROSMsg(points)));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::transformPointCloud(point_cloud_utilities::fromROSMsg(points), target_frame, x_adjust));
}

include/six_point_two_eight/make_target_models.h

作成したremoveFloorFromPointCloud2()関数を呼び出すように、呼び出し側のmake_target_models.hを修正します。

// 略。

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    // 略。
    
  public:
    void onInit() {
      // 略。
      
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          auto position = message->pose.pose.position;
          auto yaw = tf::getYaw(message->pose.pose.orientation);

          auto target_position = createPointMsg(position.x + std::cos(yaw) * radius, position.y + std::sin(yaw) * radius);
          auto target_yaw = normalizeAngle(yaw + M_PI);

          for (auto i = 0; i < step_size; ++i) {
            ROS_INFO_STREAM("Step " << (i + 1) << "/" << step_size);

            while (ros::ok()) {
              try {
                savePointCloud2File(
                  removeFloorFromPointCloud2(  // 追加。
                    getSpherePointCloud2(
                      transformPointCloud2(
                        sensor_msgs::PointCloud2ConstPtr(
                          new sensor_msgs::PointCloud2(
                            get_point_cloud_2_(createGetPointCloud2GoalMsg("points")).points)),
                        "odom"),
                      target_position,
                      radius * 0.5)));
                break;
              
              } catch (const std::exception& ex) {
                ROS_WARN_STREAM(ex.what());
              }
            }
            
            target_yaw = normalizeAngle(target_yaw + M_PI * 2 / step_size);
            
            move_base_(
              createMoveBaseGoalMsg(
                "odom",
                ros::Time::now(),
                target_position.x + std::cos(target_yaw) * radius,
                target_position.y + std::sin(target_yaw) * radius,
                normalizeAngle(target_yaw + M_PI)));
          }

          ROS_INFO_STREAM("Finished!");
          odom_subscriber_.shutdown();
        });
    }
  };
}

センサーの誤差

さて、ついに完成したプログラムを動かしてみると……ガッタガタじゃん！　分身の術みたいになってる。カバンのショルダー・ストラップが2本に分裂しちゃってるよ！

と、このようにガタガタの点群になった理由は、タイヤがスリップしたりモーターの回転数をセンサーが数え間違えたりして、オドメトリーに誤差が出るためです。ビジネス・アプリケーションでRDBMSでトランザクションな世界に慣れているとびっくりするんですけど、センサーからのデータを信用しては駄目なんです。だから、どうにかして誤差を抑えこまなければなりません。でも、ねぇ、どうやって？

3D点群を統合する

PCLを使用して、点群をレジスターする

パラメーター

Boost.Range

動作確認

おわりに